#include "png_pcd_path.hpp"
#include <fstream>
#include <sstream>
#include <queue>
#include <map>
#include <cmath>
#include <limits>
#include <algorithm>



using json = nlohmann::json;



// 构造函数
ShoeSolePathAlgorithm::ShoeSolePathAlgorithm()
    : input_cloud_(new pcl::PointCloud<pcl::PointXYZ>)
    , filtered_cloud_(new pcl::PointCloud<pcl::PointXYZ>)
    , edge_cloud_(new pcl::PointCloud<pcl::PointXYZ>)
    , inner_gluing_points_(new pcl::PointCloud<pcl::PointXYZ>)
    , outer_gluing_points_(new pcl::PointCloud<pcl::PointXYZI>)
    , sampled_outer_path_(new pcl::PointCloud<pcl::PointXYZI>)
    , sampled_inner_path_(new pcl::PointCloud<pcl::PointXYZ>)
    , transformed_outer_path_(new pcl::PointCloud<pcl::PointXYZI>)
    , transformed_inner_path_(new pcl::PointCloud<pcl::PointXYZ>)
    , knn_k_(20)
    , distance_thresh_(1.5)
{
    std::cout << "鞋底打胶路径算法初始化完成" << std::endl;
    
    // 初始化标定变换矩阵为单位矩阵
    calib_matrix_ = Eigen::Matrix4f::Identity();
    
    // 初始化姿态信息vector
    outer_path_poses_.clear();
    inner_path_poses_.clear();
    transformed_outer_path_poses_.clear();
    transformed_inner_path_poses_.clear();
    global_sweep_path_poses_.clear();
    transformed_global_sweep_path_poses_.clear();
}

// 析构函数
ShoeSolePathAlgorithm::~ShoeSolePathAlgorithm()
{
    std::cout << "鞋底打胶路径算法析构完成" << std::endl;
}

// 从参数文件加载参数
bool ShoeSolePathAlgorithm::loadParametersFromFile(const std::string& param_file)
{
    try {
        // 使用 yaml-cpp 加载 YAML 文件
        YAML::Node config = YAML::LoadFile(param_file);
        
        if (!config) {
            std::cerr << "无法加载参数文件: " << param_file << std::endl;
            return false;
        }

        std::cout << "开始加载参数文件: " << param_file << std::endl;

        // 检查是否存在 algorithm_parameters 节点
        if (config["algorithm_parameters"]) {
            YAML::Node algo_params = config["algorithm_parameters"];
            
            // 加载直通滤波器参数
            if (algo_params["passthrough_filter"]) {
                auto pf = algo_params["passthrough_filter"];
                if (pf["min_z"]) {
                    params_.passthrough_filter.min_z = pf["min_z"].as<float>();
                    std::cout << "  min_z: " << params_.passthrough_filter.min_z << std::endl;
                }
                if (pf["max_z"]) {
                    params_.passthrough_filter.max_z = pf["max_z"].as<float>();
                    std::cout << "  max_z: " << params_.passthrough_filter.max_z << std::endl;
                }
                std::cout << "加载直通滤波器参数完成" << std::endl;
            }
            
            // 加载体素滤波器参数
            if (algo_params["voxel_filter"]) {
                auto vf = algo_params["voxel_filter"];
                if (vf["voxel_size"]) {
                    params_.voxel_filter.voxel_size = vf["voxel_size"].as<float>();
                    std::cout << "  voxel_size: " << params_.voxel_filter.voxel_size << std::endl;
                }
                std::cout << "加载体素滤波器参数完成" << std::endl;
            }
            
            // 加载kNN滤波参数
            if (algo_params["knn_filter"]) {
                auto kf = algo_params["knn_filter"];
                if (kf["k"]) {
                    params_.knn_filter.k = kf["k"].as<int>();
                    std::cout << "  k: " << params_.knn_filter.k << std::endl;
                }
                if (kf["distance_threshold"]) {
                    params_.knn_filter.distance_threshold = kf["distance_threshold"].as<double>();
                    std::cout << "  distance_threshold: " << params_.knn_filter.distance_threshold << std::endl;
                }
                std::cout << "加载kNN滤波参数完成" << std::endl;
            }
            
            // 加载欧式聚类参数
            if (algo_params["euclidean_clustering"]) {
                auto ec = algo_params["euclidean_clustering"];
                if (ec["cluster_tolerance"]) {
                    params_.euclidean_clustering.cluster_tolerance = ec["cluster_tolerance"].as<double>();
                    std::cout << "  cluster_tolerance: " << params_.euclidean_clustering.cluster_tolerance << std::endl;
                }
                if (ec["min_cluster_size"]) {
                    params_.euclidean_clustering.min_cluster_size = ec["min_cluster_size"].as<int>();
                    std::cout << "  min_cluster_size: " << params_.euclidean_clustering.min_cluster_size << std::endl;
                }
                if (ec["max_cluster_size"]) {
                    params_.euclidean_clustering.max_cluster_size = ec["max_cluster_size"].as<int>();
                    std::cout << "  max_cluster_size: " << params_.euclidean_clustering.max_cluster_size << std::endl;
                }
                std::cout << "加载欧式聚类参数完成" << std::endl;
            }
            
            // 加载边缘提取参数
            if (algo_params["boundary_extraction"]) {
                auto be = algo_params["boundary_extraction"];
                if (be["step_x"]) {
                    params_.boundary_extraction.step_x = be["step_x"].as<float>();
                    std::cout << "  step_x: " << params_.boundary_extraction.step_x << std::endl;
                }
                if (be["scanline_x_range_factor"]) {
                    params_.boundary_extraction.scanline_x_range_factor = be["scanline_x_range_factor"].as<float>();
                    std::cout << "  scanline_x_range_factor: " << params_.boundary_extraction.scanline_x_range_factor << std::endl;
                }
                if (be["min_edge_distance"]) {
                    params_.boundary_extraction.min_edge_distance = be["min_edge_distance"].as<double>();
                    std::cout << "  min_edge_distance: " << params_.boundary_extraction.min_edge_distance << std::endl;
                }
                if (be["enable_distance_filter"]) {
                    params_.boundary_extraction.enable_distance_filter = be["enable_distance_filter"].as<bool>();
                    std::cout << "  enable_distance_filter: " << (params_.boundary_extraction.enable_distance_filter ? "true" : "false") << std::endl;
                }
                std::cout << "加载边缘提取参数完成" << std::endl;
            }
            
            // 加载边缘点滤波参数
            if (algo_params["edge_knn_filter"]) {
                auto ekf = algo_params["edge_knn_filter"];
                if (ekf["radius"]) {
                    params_.edge_knn_filter.radius = ekf["radius"].as<double>();
                    std::cout << "  radius: " << params_.edge_knn_filter.radius << std::endl;
                }
                if (ekf["min_neighbors"]) {
                    params_.edge_knn_filter.min_neighbors = ekf["min_neighbors"].as<int>();
                    std::cout << "  min_neighbors: " << params_.edge_knn_filter.min_neighbors << std::endl;
                }
                if (ekf["outlier_threshold"]) {
                    params_.edge_knn_filter.outlier_threshold = ekf["outlier_threshold"].as<double>();
                    std::cout << "  outlier_threshold: " << params_.edge_knn_filter.outlier_threshold << std::endl;
                }
                if (ekf["enable_filter"]) {
                    params_.edge_knn_filter.enable_filter = ekf["enable_filter"].as<bool>();
                    std::cout << "  enable_filter: " << (params_.edge_knn_filter.enable_filter ? "true" : "false") << std::endl;
                }
                if (ekf["enable_cluster_filter"]) {
                    params_.edge_knn_filter.enable_cluster_filter = ekf["enable_cluster_filter"].as<bool>();
                    std::cout << "  enable_cluster_filter: " << (params_.edge_knn_filter.enable_cluster_filter ? "true" : "false") << std::endl;
                }
                if (ekf["cluster_tolerance"]) {
                    params_.edge_knn_filter.cluster_tolerance = ekf["cluster_tolerance"].as<double>();
                    std::cout << "  cluster_tolerance: " << params_.edge_knn_filter.cluster_tolerance << std::endl;
                }
                if (ekf["min_cluster_size"]) {
                    params_.edge_knn_filter.min_cluster_size = ekf["min_cluster_size"].as<int>();
                    std::cout << "  min_cluster_size: " << params_.edge_knn_filter.min_cluster_size << std::endl;
                }
                if (ekf["max_cluster_size"]) {
                    params_.edge_knn_filter.max_cluster_size = ekf["max_cluster_size"].as<int>();
                    std::cout << "  max_cluster_size: " << params_.edge_knn_filter.max_cluster_size << std::endl;
                }
                std::cout << "加载边缘点滤波参数完成" << std::endl;
            }
            
            // 加载边缘点平滑和插值参数
            if (algo_params["edge_smoothing"]) {
                auto es = algo_params["edge_smoothing"];
                if (es["enable_smooth_filter"]) {
                    params_.edge_smoothing.enable_smooth_filter = es["enable_smooth_filter"].as<bool>();
                    std::cout << "  enable_smooth_filter: " << (params_.edge_smoothing.enable_smooth_filter ? "true" : "false") << std::endl;
                }
                if (es["smooth_neighbor_count"]) {
                    params_.edge_smoothing.smooth_neighbor_count = es["smooth_neighbor_count"].as<int>();
                    std::cout << "  smooth_neighbor_count: " << params_.edge_smoothing.smooth_neighbor_count << std::endl;
                }
                if (es["smooth_weight_sigma"]) {
                    params_.edge_smoothing.smooth_weight_sigma = es["smooth_weight_sigma"].as<double>();
                    std::cout << "  smooth_weight_sigma: " << params_.edge_smoothing.smooth_weight_sigma << std::endl;
                }
                std::cout << "加载边缘点平滑和插值参数完成" << std::endl;
            }
            
            // 加载法向量计算参数
            if (algo_params["normal_computation"]) {
                auto nc = algo_params["normal_computation"];
                if (nc["neighbor_radius"]) {
                    params_.normal_computation.neighbor_radius = nc["neighbor_radius"].as<double>();
                    std::cout << "  neighbor_radius: " << params_.normal_computation.neighbor_radius << std::endl;
                }
                if (nc["min_neighbors"]) {
                    params_.normal_computation.min_neighbors = nc["min_neighbors"].as<int>();
                    std::cout << "  min_neighbors: " << params_.normal_computation.min_neighbors << std::endl;
                }
                if (nc["enable_normal_smoothing"]) {
                    params_.normal_computation.enable_normal_smoothing = nc["enable_normal_smoothing"].as<bool>();
                    std::cout << "  enable_normal_smoothing: " << (params_.normal_computation.enable_normal_smoothing ? "true" : "false") << std::endl;
                }
                if (nc["smooth_radius"]) {
                    params_.normal_computation.smooth_radius = nc["smooth_radius"].as<double>();
                    std::cout << "  smooth_radius: " << params_.normal_computation.smooth_radius << std::endl;
                }
                if (nc["angle_threshold"]) {
                    params_.normal_computation.angle_threshold = nc["angle_threshold"].as<double>();
                    std::cout << "  angle_threshold: " << params_.normal_computation.angle_threshold << std::endl;
                }
                if (nc["smooth_iterations"]) {
                    params_.normal_computation.smooth_iterations = nc["smooth_iterations"].as<int>();
                    std::cout << "  smooth_iterations: " << params_.normal_computation.smooth_iterations << std::endl;
                }
                std::cout << "加载法向量计算参数完成" << std::endl;
            }
            
            // 加载内侧打胶点生成参数
            if (algo_params["inner_gluing_points"]) {
                auto igp = algo_params["inner_gluing_points"];
                if (igp["offset_distance"]) {
                    params_.inner_gluing_points.offset_distance = igp["offset_distance"].as<double>();
                    std::cout << "  offset_distance: " << params_.inner_gluing_points.offset_distance << std::endl;
                }
                if (igp["use_projection_height"]) {
                    params_.inner_gluing_points.use_projection_height = igp["use_projection_height"].as<bool>();
                    std::cout << "  use_projection_height: " << (params_.inner_gluing_points.use_projection_height ? "true" : "false") << std::endl;
                }
                if (igp["search_radius"]) {
                    params_.inner_gluing_points.search_radius = igp["search_radius"].as<double>();
                    std::cout << "  search_radius: " << params_.inner_gluing_points.search_radius << std::endl;
                }
                if (igp["height_offset"]) {
                    params_.inner_gluing_points.height_offset = igp["height_offset"].as<double>();
                    std::cout << "  height_offset: " << params_.inner_gluing_points.height_offset << std::endl;
                }
                std::cout << "加载内侧打胶点生成参数完成" << std::endl;
            }
            
            // 加载外部喷胶偏移调节参数
            if (algo_params["outer_spray_offset"]) {
                auto oso = algo_params["outer_spray_offset"];
                std::cout << "正在加载外部喷胶偏移调节参数..." << std::endl;
                
                if (oso["enable_offset_adjustment"]) {
                    params_.outer_spray_offset.enable_offset_adjustment = oso["enable_offset_adjustment"].as<bool>();
                    std::cout << "  enable_offset_adjustment: " << (params_.outer_spray_offset.enable_offset_adjustment ? "true" : "false") << std::endl;
                }
                if (oso["is_left_foot"]) {
                    params_.outer_spray_offset.is_left_foot = oso["is_left_foot"].as<bool>();
                    std::cout << "  is_left_foot: " << (params_.outer_spray_offset.is_left_foot ? "true" : "false") << std::endl;
                }
                
                // 加载X方向分区百分比参数
                std::string x_region_names[] = {"x_region_percent_1", "x_region_percent_2", "x_region_percent_3", "x_region_percent_4", "x_region_percent_5"};
                double* x_region_values[] = {&params_.outer_spray_offset.x_region_percent_1, &params_.outer_spray_offset.x_region_percent_2,
                                           &params_.outer_spray_offset.x_region_percent_3, &params_.outer_spray_offset.x_region_percent_4,
                                           &params_.outer_spray_offset.x_region_percent_5};
                
                double total_percent = 0.0;
                for (int i = 0; i < 5; ++i) {
                    if (oso[x_region_names[i]]) {
                        *(x_region_values[i]) = oso[x_region_names[i]].as<double>();
                        total_percent += *(x_region_values[i]);
                        // std::cout << "  " << x_region_names[i] << ": " << *(x_region_values[i]) << "%" << std::endl;
                    }
                }
                // std::cout << "  总百分比: " << total_percent << "%" << std::endl;
                if (std::abs(total_percent - 100.0) > 0.1) {
                    std::cout << "  警告: 5个分区的百分比总和不等于100%！" << std::endl;
                }
                
                // 加载连通性检测参数
                if (oso["connection_gap_threshold"]) {
                    params_.outer_spray_offset.connection_gap_threshold = oso["connection_gap_threshold"].as<double>();
                    std::cout << "  connection_gap_threshold: " << params_.outer_spray_offset.connection_gap_threshold << std::endl;
                }
                
                // 加载8个区域的Z高度调节参数
                std::string z_offset_names[] = {"z_offset_region_1", "z_offset_region_2", "z_offset_region_3", "z_offset_region_4",
                                               "z_offset_region_5", "z_offset_region_6", "z_offset_region_7", "z_offset_region_8"};
                double* z_offset_values[] = {&params_.outer_spray_offset.z_offset_region_1, &params_.outer_spray_offset.z_offset_region_2,
                                           &params_.outer_spray_offset.z_offset_region_3, &params_.outer_spray_offset.z_offset_region_4,
                                           &params_.outer_spray_offset.z_offset_region_5, &params_.outer_spray_offset.z_offset_region_6,
                                           &params_.outer_spray_offset.z_offset_region_7, &params_.outer_spray_offset.z_offset_region_8};
                
                for (int i = 0; i < 8; ++i) {
                    if (oso[z_offset_names[i]]) {
                        *(z_offset_values[i]) = oso[z_offset_names[i]].as<double>();
                        // std::cout << "  " << z_offset_names[i] << ": " << *(z_offset_values[i]) << std::endl;
                    }
                }
                
                // 加载8个区域的按法向量内移调节参数
                std::string normal_offset_names[] = {"normal_offset_region_1", "normal_offset_region_2", "normal_offset_region_3", "normal_offset_region_4",
                                                    "normal_offset_region_5", "normal_offset_region_6", "normal_offset_region_7", "normal_offset_region_8"};
                double* normal_offset_values[] = {&params_.outer_spray_offset.normal_offset_region_1, &params_.outer_spray_offset.normal_offset_region_2,
                                                 &params_.outer_spray_offset.normal_offset_region_3, &params_.outer_spray_offset.normal_offset_region_4,
                                                 &params_.outer_spray_offset.normal_offset_region_5, &params_.outer_spray_offset.normal_offset_region_6,
                                                 &params_.outer_spray_offset.normal_offset_region_7, &params_.outer_spray_offset.normal_offset_region_8};
                
                for (int i = 0; i < 8; ++i) {
                    if (oso[normal_offset_names[i]]) {
                        *(normal_offset_values[i]) = oso[normal_offset_names[i]].as<double>();
                        // std::cout << "  " << normal_offset_names[i] << ": " << *(normal_offset_values[i]) << std::endl;
                    }
                }

                // 加载8个区域的绕Y轴偏移角（度）
                std::string axis_angle_names[] = {"axis_angle_1", "axis_angle_2", "axis_angle_3", "axis_angle_4",
                                                 "axis_angle_5", "axis_angle_6", "axis_angle_7", "axis_angle_8"};
                double* axis_angle_values[] = {&params_.outer_spray_offset.axis_angle_1, &params_.outer_spray_offset.axis_angle_2,
                                               &params_.outer_spray_offset.axis_angle_3, &params_.outer_spray_offset.axis_angle_4,
                                               &params_.outer_spray_offset.axis_angle_5, &params_.outer_spray_offset.axis_angle_6,
                                               &params_.outer_spray_offset.axis_angle_7, &params_.outer_spray_offset.axis_angle_8};

                for (int i = 0; i < 8; ++i) {
                    if (oso[axis_angle_names[i]]) {
                        *(axis_angle_values[i]) = oso[axis_angle_names[i]].as<double>();
                        // std::cout << "  " << axis_angle_names[i] << ": " << *(axis_angle_values[i]) << "度" << std::endl;
                    }
                }
                
                std::cout << "加载外部喷胶偏移调节参数完成" << std::endl;
            }
            
            // 加载路径采样参数
            if (algo_params["path_sampling"]) {
                auto ps = algo_params["path_sampling"];
                if (ps["outer_points"]) {
                    params_.path_sampling.outer_points = ps["outer_points"].as<int>();
                    std::cout << "  outer_points: " << params_.path_sampling.outer_points << std::endl;
                }
                if (ps["inner_points"]) {
                    params_.path_sampling.inner_points = ps["inner_points"].as<int>();
                    std::cout << "  inner_points: " << params_.path_sampling.inner_points << std::endl;
                }
                std::cout << "加载路径采样参数完成" << std::endl;
            }
        } else {
            std::cout << "未找到 algorithm_parameters 节点，将使用默认参数" << std::endl;
        }

        // 加载标定矩阵
        if (config["calib"]) {
            YAML::Node calib_node = config["calib"];
            
            if (calib_node.IsSequence() && calib_node.size() >= 4) {
                // 矩阵以序列形式存储，每行为一个子序列
                for (int i = 0; i < 4 && i < calib_node.size(); ++i) {
                    YAML::Node row = calib_node[i];
                    if (row.IsSequence() && row.size() >= 4) {
                        for (int j = 0; j < 4 && j < row.size(); ++j) {
                            calib_matrix_(i, j) = row[j].as<float>();
                        }
                    }
                }
                std::cout << "从序列格式加载标定矩阵完成" << std::endl;
            } else if (calib_node.IsScalar()) {
                // 矩阵以多行字符串形式存储
                std::string calib_str = calib_node.as<std::string>();
                std::istringstream iss(calib_str);
                std::string line;
                int row = 0;
                
                while (std::getline(iss, line) && row < 4) {
                    // 跳过空行
                    if (line.empty()) continue;
                    
                    std::istringstream line_stream(line);
                    std::vector<float> row_values;
                    float value;
                    
                    // 读取一行中的所有数值
                    while (line_stream >> value) {
                        row_values.push_back(value);
                    }
                    
                    // 如果这一行有4个数值，则填充到矩阵中
                    if (row_values.size() == 4) {
                        for (int j = 0; j < 4; ++j) {
                            calib_matrix_(row, j) = row_values[j];
                        }
                        row++;
                    }
                }
                std::cout << "从字符串格式加载标定矩阵完成" << std::endl;
            } else {
                std::cout << "警告: 标定矩阵格式不正确，使用单位矩阵" << std::endl;
                calib_matrix_ = Eigen::Matrix4f::Identity();
            }
        } else {
            std::cout << "未找到标定矩阵，使用单位矩阵" << std::endl;
            calib_matrix_ = Eigen::Matrix4f::Identity();
        }
        
        // 同时更新兼容性变量
        knn_k_ = params_.knn_filter.k;
        distance_thresh_ = params_.knn_filter.distance_threshold;
        
        std::cout << "成功加载参数文件: " << param_file << std::endl;
        
        // 打印加载的标定矩阵
        std::cout << "加载的标定变换矩阵:" << std::endl;
        std::cout << calib_matrix_ << std::endl;
        
        return true;
        
    } catch (const YAML::Exception& e) {
        std::cerr << "YAML解析错误: " << e.what() << std::endl;
        std::cerr << "参数文件: " << param_file << std::endl;
        return false;
    } catch (const std::exception& e) {
        std::cerr << "加载参数文件时发生错误: " << e.what() << std::endl;
        return false;
    }
}

// 打印当前参数设置
void ShoeSolePathAlgorithm::printParameters() const
{
    std::cout << "\n=== 当前算法参数设置 ===" << std::endl;
    std::cout << "直通滤波器:" << std::endl;
    std::cout << "  min_z: " << params_.passthrough_filter.min_z << std::endl;
    std::cout << "  max_z: " << params_.passthrough_filter.max_z << std::endl;
    
    std::cout << "体素滤波器:" << std::endl;
    std::cout << "  voxel_size: " << params_.voxel_filter.voxel_size << std::endl;
    
    std::cout << "kNN滤波器:" << std::endl;
    std::cout << "  k: " << params_.knn_filter.k << std::endl;
    std::cout << "  distance_threshold: " << params_.knn_filter.distance_threshold << std::endl;
    
    std::cout << "欧式聚类:" << std::endl;
    std::cout << "  cluster_tolerance: " << params_.euclidean_clustering.cluster_tolerance << std::endl;
    std::cout << "  min_cluster_size: " << params_.euclidean_clustering.min_cluster_size << std::endl;
    std::cout << "  max_cluster_size: " << params_.euclidean_clustering.max_cluster_size << std::endl;
    
    std::cout << "边缘提取:" << std::endl;
    std::cout << "  step_x: " << params_.boundary_extraction.step_x << std::endl;
    std::cout << "  scanline_x_range_factor: " << params_.boundary_extraction.scanline_x_range_factor << std::endl;
    std::cout << "  min_edge_distance: " << params_.boundary_extraction.min_edge_distance << std::endl;
    std::cout << "  enable_distance_filter: " << (params_.boundary_extraction.enable_distance_filter ? "true" : "false") << std::endl;
    
    std::cout << "边缘点Z高度统计滤波:" << std::endl;
    std::cout << "  radius: " << params_.edge_knn_filter.radius << std::endl;
    std::cout << "  min_neighbors: " << params_.edge_knn_filter.min_neighbors << std::endl;
    std::cout << "  outlier_threshold (std_dev_multiplier): " << params_.edge_knn_filter.outlier_threshold << std::endl;
    std::cout << "  enable_filter: " << (params_.edge_knn_filter.enable_filter ? "true" : "false") << std::endl;
    std::cout << "  enable_cluster_filter: " << (params_.edge_knn_filter.enable_cluster_filter ? "true" : "false") << std::endl;
    std::cout << "  cluster_tolerance: " << params_.edge_knn_filter.cluster_tolerance << std::endl;
    std::cout << "  min_cluster_size: " << params_.edge_knn_filter.min_cluster_size << std::endl;
    std::cout << "  max_cluster_size: " << params_.edge_knn_filter.max_cluster_size << std::endl;
    
    std::cout << "边缘点平滑和插值:" << std::endl;
    std::cout << "  enable_smooth_filter: " << (params_.edge_smoothing.enable_smooth_filter ? "true" : "false") << std::endl;
    std::cout << "  smooth_neighbor_count: " << params_.edge_smoothing.smooth_neighbor_count << std::endl;
    std::cout << "  smooth_weight_sigma: " << params_.edge_smoothing.smooth_weight_sigma << std::endl;

    
    std::cout << "法向量计算:" << std::endl;
    std::cout << "  neighbor_radius: " << params_.normal_computation.neighbor_radius << std::endl;
    std::cout << "  min_neighbors: " << params_.normal_computation.min_neighbors << std::endl;
    std::cout << "  enable_normal_smoothing: " << (params_.normal_computation.enable_normal_smoothing ? "true" : "false") << std::endl;
    std::cout << "  smooth_radius: " << params_.normal_computation.smooth_radius << std::endl;
    std::cout << "  angle_threshold: " << params_.normal_computation.angle_threshold << std::endl;
    std::cout << "  smooth_iterations: " << params_.normal_computation.smooth_iterations << std::endl;
    
    std::cout << "内侧打胶点:" << std::endl;
    std::cout << "  offset_distance: " << params_.inner_gluing_points.offset_distance << std::endl;
    std::cout << "  use_projection_height: " << (params_.inner_gluing_points.use_projection_height ? "true" : "false") << std::endl;
    std::cout << "  search_radius: " << params_.inner_gluing_points.search_radius << std::endl;
    std::cout << "  height_offset: " << params_.inner_gluing_points.height_offset << std::endl;
    // std::cout << "  enable_z_smoothing: " << (params_.inner_gluing_points.enable_z_smoothing ? "true" : "false") << std::endl;
    // std::cout << "  z_smooth_radius: " << params_.inner_gluing_points.z_smooth_radius << std::endl;
    // std::cout << "  z_smooth_iterations: " << params_.inner_gluing_points.z_smooth_iterations << std::endl;
    // std::cout << "  enable_z_outlier_filter: " << (params_.inner_gluing_points.enable_z_outlier_filter ? "true" : "false") << std::endl;
    // std::cout << "  z_outlier_threshold: " << params_.inner_gluing_points.z_outlier_threshold << std::endl;
    // std::cout << "  z_filter_radius: " << params_.inner_gluing_points.z_filter_radius << std::endl;
    
    std::cout << "外部喷胶偏移调节:" << std::endl;
    std::cout << "  enable_offset_adjustment: " << (params_.outer_spray_offset.enable_offset_adjustment ? "true" : "false") << std::endl;
    std::cout << "  is_left_foot: " << (params_.outer_spray_offset.is_left_foot ? "true" : "false") << std::endl;
    std::cout << "  X方向5个分区占用百分比:" << std::endl;
    std::cout << "    x_region_percent_1: " << params_.outer_spray_offset.x_region_percent_1 << "%" << std::endl;
    std::cout << "    x_region_percent_2: " << params_.outer_spray_offset.x_region_percent_2 << "%" << std::endl;
    std::cout << "    x_region_percent_3: " << params_.outer_spray_offset.x_region_percent_3 << "%" << std::endl;
    std::cout << "    x_region_percent_4: " << params_.outer_spray_offset.x_region_percent_4 << "%" << std::endl;
    std::cout << "    x_region_percent_5: " << params_.outer_spray_offset.x_region_percent_5 << "%" << std::endl;
    double total_percent = params_.outer_spray_offset.x_region_percent_1 + params_.outer_spray_offset.x_region_percent_2 + 
                          params_.outer_spray_offset.x_region_percent_3 + params_.outer_spray_offset.x_region_percent_4 + 
                          params_.outer_spray_offset.x_region_percent_5;
    std::cout << "    总和: " << total_percent << "%" << std::endl;
    std::cout << "  connection_gap_threshold: " << params_.outer_spray_offset.connection_gap_threshold << std::endl;
    std::cout << "  Z高度调节参数:" << std::endl;
    std::cout << "    region_1: " << params_.outer_spray_offset.z_offset_region_1 << std::endl;
    std::cout << "    region_2: " << params_.outer_spray_offset.z_offset_region_2 << std::endl;
    std::cout << "    region_3: " << params_.outer_spray_offset.z_offset_region_3 << std::endl;
    std::cout << "    region_4: " << params_.outer_spray_offset.z_offset_region_4 << std::endl;
    std::cout << "    region_5: " << params_.outer_spray_offset.z_offset_region_5 << std::endl;
    std::cout << "    region_6: " << params_.outer_spray_offset.z_offset_region_6 << std::endl;
    std::cout << "    region_7: " << params_.outer_spray_offset.z_offset_region_7 << std::endl;
    std::cout << "    region_8: " << params_.outer_spray_offset.z_offset_region_8 << std::endl;
    std::cout << "  法向量内移调节参数:" << std::endl;
    std::cout << "    region_1: " << params_.outer_spray_offset.normal_offset_region_1 << std::endl;
    std::cout << "    region_2: " << params_.outer_spray_offset.normal_offset_region_2 << std::endl;
    std::cout << "    region_3: " << params_.outer_spray_offset.normal_offset_region_3 << std::endl;
    std::cout << "    region_4: " << params_.outer_spray_offset.normal_offset_region_4 << std::endl;
    std::cout << "    region_5: " << params_.outer_spray_offset.normal_offset_region_5 << std::endl;
    std::cout << "    region_6: " << params_.outer_spray_offset.normal_offset_region_6 << std::endl;
    std::cout << "    region_7: " << params_.outer_spray_offset.normal_offset_region_7 << std::endl;
    std::cout << "    region_8: " << params_.outer_spray_offset.normal_offset_region_8 << std::endl;
    std::cout << "  绕Y轴角度偏移参数（度）:" << std::endl;
    std::cout << "    region_1: " << params_.outer_spray_offset.axis_angle_1 << "°" << std::endl;
    std::cout << "    region_2: " << params_.outer_spray_offset.axis_angle_2 << "°" << std::endl;
    std::cout << "    region_3: " << params_.outer_spray_offset.axis_angle_3 << "°" << std::endl;
    std::cout << "    region_4: " << params_.outer_spray_offset.axis_angle_4 << "°" << std::endl;
    std::cout << "    region_5: " << params_.outer_spray_offset.axis_angle_5 << "°" << std::endl;
    std::cout << "    region_6: " << params_.outer_spray_offset.axis_angle_6 << "°" << std::endl;
    std::cout << "    region_7: " << params_.outer_spray_offset.axis_angle_7 << "°" << std::endl;
    std::cout << "    region_8: " << params_.outer_spray_offset.axis_angle_8 << "°" << std::endl;
    
    std::cout << "路径采样:" << std::endl;
    std::cout << "  outer_points: " << params_.path_sampling.outer_points << std::endl;
    std::cout << "  inner_points: " << params_.path_sampling.inner_points << std::endl;
    std::cout << "========================\n" << std::endl;
}

// 加载点云文件
bool ShoeSolePathAlgorithm::loadPointCloud(const std::string& filename)
{
    if (pcl::io::loadPCDFile<pcl::PointXYZ>(filename, *input_cloud_) == -1)
    {
        std::cerr << "无法加载点云文件: " << filename << std::endl;
        return false;
    }
    
    std::cout << "成功加载点云文件: " << filename << std::endl;
    std::cout << "点云包含 " << input_cloud_->points.size() << " 个点" << std::endl;
    return true;
}

// 应用直通滤波器（整合版本，支持参数文件和直接传参）
bool ShoeSolePathAlgorithm::applyPassThroughFilter()
{
    return applyPassThroughFilter(params_.passthrough_filter.min_z, params_.passthrough_filter.max_z);
}

bool ShoeSolePathAlgorithm::applyPassThroughFilter(float min_z, float max_z)
{
    if (input_cloud_->empty())
    {
        std::cerr << "输入点云为空，无法应用直通滤波" << std::endl;
        return false;
    }
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(input_cloud_);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(min_z, max_z);//-155, -60
    pass.setNegative(false);
    pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pass.filter(*temp_cloud);
    
    // 将结果复制到filtered_cloud_
    *filtered_cloud_ = *temp_cloud;
    
    std::cout << "直通滤波完成: Z轴范围[" << min_z << ", " << max_z << "]" << std::endl;
    std::cout << "滤波后点云包含 " << temp_cloud->points.size() << " 个点" << std::endl;
    return true;
}

// 应用体素滤波器（整合版本，支持参数文件和直接传参）
bool ShoeSolePathAlgorithm::applyVoxelFilter()
{
    return applyVoxelFilter(params_.voxel_filter.voxel_size);
}

bool ShoeSolePathAlgorithm::applyVoxelFilter(float voxel_size)
{
    if (filtered_cloud_->empty())
    {
        std::cerr << "滤波点云为空，无法应用体素滤波" << std::endl;
        return false;
    }

    pcl::VoxelGrid<pcl::PointXYZ> voxel_filter;
    voxel_filter.setInputCloud(filtered_cloud_);
    voxel_filter.setLeafSize(voxel_size, voxel_size, voxel_size);
    
    pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    voxel_filter.filter(*temp_cloud);
    
    // 将结果复制到filtered_cloud_
    *filtered_cloud_ = *temp_cloud;
    
    std::cout << "体素滤波完成: 体素大小 = " << voxel_size << std::endl;
    std::cout << "降采样后点云包含 " << filtered_cloud_->points.size() << " 个点" << std::endl;
    
    // 打印点云的基本统计信息
    if (!filtered_cloud_->empty())
    {
        pcl::PointXYZ min_pt, max_pt;
        pcl::getMinMax3D(*filtered_cloud_, min_pt, max_pt);
        std::cout << "点云范围: X[" << min_pt.x << ", " << max_pt.x << "], "
                  << "Y[" << min_pt.y << ", " << max_pt.y << "], "
                  << "Z[" << min_pt.z << ", " << max_pt.z << "]" << std::endl;
    }
    
    return true;
}

// kNN滤波（整合版本，支持参数文件和直接传参）
bool ShoeSolePathAlgorithm::applyKNNFilter()
{
    return applyKNNFilter(params_.knn_filter.k, params_.knn_filter.distance_threshold);
}

// 基于kNN的邻域滤波算法（整合版本）
bool ShoeSolePathAlgorithm::applyKNNFilter(int k, double distance_threshold)
{
    if (filtered_cloud_->empty())
    {
        std::cerr << "滤波点云为空，无法应用kNN滤波" << std::endl;
        return false;
    }

    knn_k_ = k;
    distance_thresh_ = distance_threshold;

    // 构建KDTree
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    kdtree.setInputCloud(filtered_cloud_);

    pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    
    std::cout << "开始kNN邻域滤波，k=" << k << ", 距离阈值=" << distance_threshold << std::endl;
    
    // 计算一些样本点的平均距离来估算合适的阈值
    std::vector<double> sample_distances;
    int sample_count = std::min(100, (int)filtered_cloud_->points.size());
    
    for (int i = 0; i < sample_count; ++i)
    {
        const pcl::PointXYZ& point = filtered_cloud_->points[i];
        double kth_distance = computeKthNearestDistance(kdtree, point, k);
        if (kth_distance != std::numeric_limits<double>::max())
        {
            sample_distances.push_back(kth_distance);
        }
    }
    
    if (!sample_distances.empty())
    {
        double avg_distance = 0.0;
        for (double d : sample_distances)
        {
            avg_distance += d;
        }
        avg_distance /= sample_distances.size();
        std::cout << "样本平均第" << k << "近邻距离: " << avg_distance << std::endl;
        
        // 如果设置的阈值明显小于平均距离，建议调整
        if (distance_threshold < avg_distance * 0.5)
        {
            std::cout << "警告: 当前阈值(" << distance_threshold 
                     << ")可能过小，建议设置为: " << avg_distance * 1.5 << std::endl;
        }
    }
    
    int valid_points = 0;
    for (size_t i = 0; i < filtered_cloud_->points.size(); ++i)
    {
        const pcl::PointXYZ& point = filtered_cloud_->points[i];
        
        // 计算点到其第k个最近邻的距离
        double kth_distance = computeKthNearestDistance(kdtree, point, k);
        
        // 如果距离小于阈值，则保留该点
        if (kth_distance <= distance_threshold)
        {
            temp_cloud->points.push_back(point);
            valid_points++;
        }
    }
    
    temp_cloud->width = temp_cloud->points.size();
    temp_cloud->height = 1;
    temp_cloud->is_dense = true;
    
    // 将结果复制到filtered_cloud_
    *filtered_cloud_ = *temp_cloud;
    
    std::cout << "kNN滤波完成，保留 " << valid_points << " 个有效点" << std::endl;
    return true;
}

// 欧式聚类（整合版本，支持参数文件和直接传参）
bool ShoeSolePathAlgorithm::applyEuclideanClustering()
{
    return applyEuclideanClustering(params_.euclidean_clustering.cluster_tolerance,
                                    params_.euclidean_clustering.min_cluster_size,
                                    params_.euclidean_clustering.max_cluster_size);
}

// 应用欧式聚类算法，只保留最大聚类（整合版本）
bool ShoeSolePathAlgorithm::applyEuclideanClustering(double cluster_tolerance, 
                                                    int min_cluster_size, 
                                                    int max_cluster_size)
{
    if (filtered_cloud_->empty())
    {
        std::cerr << "滤波点云为空，无法应用欧式聚类" << std::endl;
        return false;
    }

    std::cout << "开始欧式聚类，容忍度=" << cluster_tolerance 
              << ", 最小聚类=" << min_cluster_size 
              << ", 最大聚类=" << max_cluster_size << std::endl;

    // 创建KdTree对象用于搜索
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud(filtered_cloud_);

    // 欧式聚类对象
    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    ec.setClusterTolerance(cluster_tolerance);
    ec.setMinClusterSize(min_cluster_size);
    ec.setMaxClusterSize(max_cluster_size);
    ec.setSearchMethod(tree);
    ec.setInputCloud(filtered_cloud_);
    ec.extract(cluster_indices);

    if (cluster_indices.empty())
    {
        std::cerr << "未找到符合条件的聚类" << std::endl;
        return false;
    }

    std::cout << "找到 " << cluster_indices.size() << " 个聚类" << std::endl;

    // 找到最大的聚类
    size_t max_cluster_size_found = 0;
    int max_cluster_idx = 0;
    
    for (size_t i = 0; i < cluster_indices.size(); ++i)
    {
        size_t cluster_size = cluster_indices[i].indices.size();
        std::cout << "聚类 " << i << " 包含 " << cluster_size << " 个点" << std::endl;
        
        if (cluster_size > max_cluster_size_found)
        {
            max_cluster_size_found = cluster_size;
            max_cluster_idx = i;
        }
    }

    std::cout << "选择最大聚类 " << max_cluster_idx << "，包含 " << max_cluster_size_found << " 个点" << std::endl;

    // 提取最大聚类的点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr largest_cluster_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    
    for (const auto& idx : cluster_indices[max_cluster_idx].indices)
    {
        largest_cluster_cloud->points.push_back((*filtered_cloud_)[idx]);
    }
    
    largest_cluster_cloud->width = largest_cluster_cloud->points.size();
    largest_cluster_cloud->height = 1;
    largest_cluster_cloud->is_dense = true;

    // 用最大聚类替换filtered_cloud_
    *filtered_cloud_ = *largest_cluster_cloud;

    std::cout << "欧式聚类完成，保留最大聚类 " << filtered_cloud_->points.size() << " 个点" << std::endl;
    return true;
}

// 计算点到其第k个最近邻的距离
double ShoeSolePathAlgorithm::computeKthNearestDistance(const pcl::KdTreeFLANN<pcl::PointXYZ>& kdtree, 
                                                      const pcl::PointXYZ& point, int k)
{
    std::vector<int> pointIdxNKNSearch(k + 1);  // +1因为包含自身
    std::vector<float> pointNKNSquaredDistance(k + 1);

    if (kdtree.nearestKSearch(point, k + 1, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
    {
        // 返回第k个最近邻的距离（跳过自身）
        if (pointNKNSquaredDistance.size() > k)
        {
            return std::sqrt(pointNKNSquaredDistance[k]);
        }
    }
    
    return std::numeric_limits<double>::max();
}

// 边缘提取（整合版本，支持参数文件和直接传参）
bool ShoeSolePathAlgorithm::extractSoleBoundary()
{
    return extractSoleBoundary(params_.boundary_extraction.step_x);
}

// 双边极大值边缘提取算法（整合版本）
bool ShoeSolePathAlgorithm::extractSoleBoundary(float step_x)
{
    if (filtered_cloud_->empty())
    {
        std::cerr << "滤波点云为空，无法提取边缘" << std::endl;
        return false;
    }

    edge_cloud_->clear();

    std::cout << "开始双边极大值边缘提取（X方向 + Y方向）..." << std::endl;
    if (params_.boundary_extraction.enable_distance_filter) {
        std::cout << "启用距离过滤，最小边缘点距离: " << params_.boundary_extraction.min_edge_distance << std::endl;
    }

    // 按X坐标排序点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr sorted_cloud = sortPointsByX(filtered_cloud_);

    // 找到X坐标的范围
    float min_x = sorted_cloud->points.front().x;
    float max_x = sorted_cloud->points.back().x;
    
    // 找到Y坐标的范围
    float min_y = sorted_cloud->points.front().y;
    float max_y = sorted_cloud->points.back().y;



    int min_x_S = 15;
    int max_x_S = 8;



    std::cout << "X坐标范围: [" << min_x << ", " << max_x << "]" << std::endl;
    std::cout << "Y坐标范围: [" << min_y << ", " << max_y << "]" << std::endl;
    std::cout << "扫描线X方向采样范围因子: " << params_.boundary_extraction.scanline_x_range_factor << std::endl;
    std::cout << "实际扫描线采样宽度: " << step_x << std::endl;

    // =========================== X方向边缘提取 ===========================
    std::cout << "开始X方向边缘提取..." << std::endl;
    
    float scanline_count = (max_x-min_x)/step_x;//总步长
    std::cout<<"扫描线数量: "<<scanline_count<<std::endl;

    std::vector<pcl::PointXYZ,Eigen::aligned_allocator<pcl::PointXYZ>> left_edge_points;  
    std::vector<pcl::PointXYZ,Eigen::aligned_allocator<pcl::PointXYZ>> right_edge_points; 

    std::vector<pcl::PointXYZ,Eigen::aligned_allocator<pcl::PointXYZ>> up_edge_points;  
    std::vector<pcl::PointXYZ,Eigen::aligned_allocator<pcl::PointXYZ>> down_edge_points; 


    for (int i = 0; i < scanline_count; ++i)
    {
        float current_x = min_x + i * step_x;
        float next_x = current_x + params_.boundary_extraction.scanline_x_range_factor;//步长

        // 收集当前扫描线范围内的点
        std::vector<pcl::PointXYZ> scanline_points;
        for (const auto& point : sorted_cloud->points)
        {
            if (point.x >= current_x && point.x < next_x && point.x <= (max_x - max_x_S) && point.x >= (min_x_S + min_x))
            {
                scanline_points.push_back(point);
            }
        }
        
        if (scanline_points.empty())
            continue;

        // 按Y坐标排序
        std::sort(scanline_points.begin(), scanline_points.end(),
                 [](const pcl::PointXYZ& a, const pcl::PointXYZ& b) {
                     return a.y < b.y;
                 });

        // 找到Y坐标的中点
        size_t mid_idx = scanline_points.size() / 2;

        // 左部分：从上到下找第一个出现的最大值点A
        std::vector<pcl::PointXYZ> upper_half(scanline_points.begin() + mid_idx, scanline_points.end());
        // 右边部分：从下到上找第一个出现的最大值点B  
        std::vector<pcl::PointXYZ> lower_half(scanline_points.begin(), scanline_points.begin() + mid_idx + 1);

        // 检查上下两部分是否都有点
        if (upper_half.empty() || lower_half.empty())
        {
                continue;
        }

        pcl::PointXYZ point_A = findExtremePoint(upper_half, true, true);//从头到尾
        pcl::PointXYZ point_B = findExtremePoint(lower_half, true, false);//从尾到头

        // 如果启用距离过滤，检查两点间的欧式距离
        if (params_.boundary_extraction.enable_distance_filter)
        {
            double distance = std::sqrt(
                std::pow(point_A.x - point_B.x, 2) + 
                std::pow(point_A.y - point_B.y, 2) + 
                std::pow(point_A.z - point_B.z, 2)
            );
            
            if (distance < params_.boundary_extraction.min_edge_distance)
            {
                // 距离太近，认为有问题，过滤掉
                continue;
            }
        }

        // 分别添加到上下边缘点向量
        left_edge_points.push_back(point_A);  // 上边缘点（Y值较大）
        right_edge_points.push_back(point_B);  // 下边缘点（Y值较小）
    }
////////////////////////////////////////////////////////////////////////////////////


    int step_y = 4;
    int out_step_y = 4;


    for (int i = 0; i < 25; ++i)
    {
        float current_y_up = max_y-20 + i * step_y;
        float next_y_up = current_y_up + out_step_y;//步长

        float current_y_down = min_y-20 + i * step_y;
        float next_y_down = current_y_down + out_step_y;//步长


        // 收集当前扫描线范围内的点
        std::vector<pcl::PointXYZ,Eigen::aligned_allocator<pcl::PointXYZ>> scanline_points;
        for (const auto& point : sorted_cloud->points)
        {
            if(((point.x<max_x+20 && point.x>max_x - 20)&&(point.y<max_y+20 && point.y>max_y - 20)) || ((point.x>min_x -25 && point.x<min_x +25)&& (point.y>min_y -15 && point.y<min_y +15))){
                if((point.y>current_y_up && point.y < next_y_up)||(point.y>current_y_down && point.y < next_y_down)){
                    scanline_points.push_back(point);
                }
            }
        }

        if (scanline_points.empty())
            continue;

        // 按x坐标排序
        std::sort(scanline_points.begin(), scanline_points.end(),
                 [](const pcl::PointXYZ& a, const pcl::PointXYZ& b) {
                     return a.x < b.x;
                 });

        // 找到x坐标的中点
        size_t mid_idx = scanline_points.size() / 2;

        //上半段
        std::vector<pcl::PointXYZ> lower_half(scanline_points.begin() + mid_idx, scanline_points.end());
        // 下半段
        std::vector<pcl::PointXYZ> upper_half(scanline_points.begin(), scanline_points.begin() + mid_idx + 1);

        // 检查上下两部分是否都有点
        if (upper_half.empty() || lower_half.empty())
        {
            continue;
        }


        pcl::PointXYZ point_A = findExtremePoint(upper_half, true, true);//从头到尾
        pcl::PointXYZ point_B = findExtremePoint(lower_half, true, false);//从尾到头

        // 分别添加到上下边缘点向量
        left_edge_points.push_back(point_A);  // 上边缘点（Y值较大）
        right_edge_points.push_back(point_B);  // 下边缘点（Y值较小）

        // std::cout<<"iteration: "<<i<<std::endl;
        // std::cout<<"PointA: "<<point_A<<std::endl;
        // std::cout<<"PointB: "<<point_B<<std::endl;

    }

////////////////////////////////////////////////////////////////////////////////////
    // 定义半径滤波参数
    float xy_radius = 10.0f;  // XY平面半径阈值，单位：mm
    float z_radius = 5.0f;   // Z高度半径阈值，单位：mm
    int min_neighbors_xy = 2;  // XY平面最少邻居数
    int min_neighbors_z = 2;   // Z方向最少邻居数
    
    std::vector<pcl::PointXYZ,Eigen::aligned_allocator<pcl::PointXYZ>> filtered_left_edge_points;
    std::vector<pcl::PointXYZ,Eigen::aligned_allocator<pcl::PointXYZ>> filtered_right_edge_points;

    //左端
    if (!left_edge_points.empty()) {
        // 将vector转换为PCL点云以使用KdTree
        pcl::PointCloud<pcl::PointXYZ>::Ptr left_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        left_cloud->points = left_edge_points;
        left_cloud->width = left_edge_points.size();
        left_cloud->height = 1;
        left_cloud->is_dense = true;
        
        // 构建KdTree
        pcl::KdTreeFLANN<pcl::PointXYZ> kdtree_left;
        kdtree_left.setInputCloud(left_cloud);
        
        for (size_t i = 0; i < left_edge_points.size(); ++i) {
            const auto& point = left_edge_points[i];
            
            // XY平面半径搜索
            std::vector<int> xy_indices;
            std::vector<float> xy_distances;
            int xy_neighbors = kdtree_left.radiusSearch(point, xy_radius, xy_indices, xy_distances);
            
            // 在XY邻域中统计Z方向邻居数
            int z_neighbors = 0;
            for (int idx : xy_indices) {
                if (idx == static_cast<int>(i)) continue;  // 排除自身
                float z_dist = std::abs(point.z - left_edge_points[idx].z);
                if (z_dist <= z_radius) {
                    z_neighbors++;
                }
            }
            
            // 同时满足XY平面和Z方向的邻居数要求才保留
            // xy_neighbors包含自身，所以需要减1
            if ((xy_neighbors - 1) >= min_neighbors_xy && z_neighbors >= min_neighbors_z) {
                filtered_left_edge_points.push_back(point);
            }
        }
    }
        
    //右端
    if (!right_edge_points.empty()) {
        // 将vector转换为PCL点云以使用KdTree
        pcl::PointCloud<pcl::PointXYZ>::Ptr right_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        right_cloud->points = right_edge_points;
        right_cloud->width = right_edge_points.size();
        right_cloud->height = 1;
        right_cloud->is_dense = true;
        
        // 构建KdTree
        pcl::KdTreeFLANN<pcl::PointXYZ> kdtree_right;
        kdtree_right.setInputCloud(right_cloud);
        
        for (size_t i = 0; i < right_edge_points.size(); ++i) {
            const auto& point = right_edge_points[i];
            
            // XY平面半径搜索
            std::vector<int> xy_indices;
            std::vector<float> xy_distances;
            int xy_neighbors = kdtree_right.radiusSearch(point, xy_radius, xy_indices, xy_distances);
            
            // 在XY邻域中统计Z方向邻居数
            int z_neighbors = 0;
            for (int idx : xy_indices) {
                if (idx == static_cast<int>(i)) continue;  // 排除自身
                float z_dist = std::abs(point.z - right_edge_points[idx].z);
                if (z_dist <= z_radius) {
                    z_neighbors++;
                }
            }
            
            // 同时满足XY平面和Z方向的邻居数要求才保留
            // xy_neighbors包含自身，所以需要减1
            if ((xy_neighbors - 1) >= min_neighbors_xy && z_neighbors >= min_neighbors_z) {
                filtered_right_edge_points.push_back(point);
            }
        }
    }
    
    std::cout << "右边缘点过滤: 原始 " << right_edge_points.size() 
              << " 点 -> 过滤后 " << filtered_right_edge_points.size() << " 点" << std::endl;
    
    // 使用过滤后的边缘点
    left_edge_points = filtered_left_edge_points;
    right_edge_points = filtered_right_edge_points;
    
    for (const auto& point : left_edge_points){
        edge_cloud_->points.push_back(point);
    }
    for (auto it = right_edge_points.rbegin(); it != right_edge_points.rend(); ++it){
        edge_cloud_->points.push_back(*it);
    }



    
    edge_cloud_->width = edge_cloud_->points.size();
    edge_cloud_->height = 1;
    edge_cloud_->is_dense = true;

    std::cout << "边缘提取完成，提取到 " << edge_cloud_->points.size() << " 个边缘点" << std::endl;
    std::cout << "其中上边缘点 " << left_edge_points.size() << " 个，下边缘点 " << right_edge_points.size() << " 个" << std::endl;
    std::cout << "边缘点已按顺时针方向排列" << std::endl;
    return true;
}

// 边缘点基于聚类和Z高度的统计滤波（使用参数文件）
bool ShoeSolePathAlgorithm::applyEdgeKNNFilter()
{
    // 第1步：基于聚类进行滤波，去除异常的路径点
    if (params_.edge_knn_filter.enable_cluster_filter)
    {
        
        std::cout << "步骤1: 执行聚类滤波去除异常路径点..." << std::endl;
        std::cout << "聚类容忍度: " << params_.edge_knn_filter.cluster_tolerance 
                  << ", 最小聚类大小: " << params_.edge_knn_filter.min_cluster_size
                  << ", 最大聚类大小: " << params_.edge_knn_filter.max_cluster_size << std::endl;
        
        // 创建KdTree对象用于聚类搜索
        pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
        tree->setInputCloud(edge_cloud_);

        // 欧式聚类对象
        std::vector<pcl::PointIndices> cluster_indices;
        pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
        ec.setClusterTolerance(params_.edge_knn_filter.cluster_tolerance);
        ec.setMinClusterSize(params_.edge_knn_filter.min_cluster_size);
        ec.setMaxClusterSize(params_.edge_knn_filter.max_cluster_size);
        ec.setSearchMethod(tree);
        ec.setInputCloud(edge_cloud_);
        ec.extract(cluster_indices);

        if (cluster_indices.empty())
        {
            std::cerr << "警告: 聚类后未找到符合条件的聚类，保留原始边缘点" << std::endl;
        }
        else
        {
            std::cout << "找到 " << cluster_indices.size() << " 个边缘聚类" << std::endl;
            
            // 收集所有有效聚类中的点
            pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
            int total_clustered_points = 0;
            
            for (size_t i = 0; i < cluster_indices.size(); ++i)
            {
                size_t cluster_size = cluster_indices[i].indices.size();
                std::cout << "聚类 " << i << " 包含 " << cluster_size << " 个点" << std::endl;
                if(cluster_size<params_.edge_knn_filter.cluster_tolerance)
                {
                    std::cout << "聚类 " << i << " 点数过少，跳过" << std::endl;
                    continue;
                }
                // 将聚类中的所有点添加到结果点云中
                for (const auto& idx : cluster_indices[i].indices)
                {
                    clustered_cloud->points.push_back((*edge_cloud_)[idx]);
                    total_clustered_points++;
                }
            }
            
            clustered_cloud->width = clustered_cloud->points.size();
            clustered_cloud->height = 1;
            clustered_cloud->is_dense = true;

            // 更新边缘点云
            *edge_cloud_ = *clustered_cloud;
            
            int removed_points = edge_cloud_->points.size() - total_clustered_points;
            std::cout << "聚类滤波完成，保留 " << total_clustered_points << " 个聚类点，去除了 " << removed_points << " 个异常点" << std::endl;
        }
        
        // 如果聚类后点云为空，直接返回
        if (edge_cloud_->empty())
        {
            std::cerr << "聚类后边缘点云为空，无法继续处理" << std::endl;
            return false;
        }
    }
    if (!params_.edge_knn_filter.enable_filter)
    {
        std::cout << "边缘点统计滤波已禁用，跳过滤波步骤" << std::endl;
        return true;
    }
    
    if (edge_cloud_->empty())
    {
        std::cerr << "边缘点云为空，无法应用统计滤波" << std::endl;
        return false;
    }
    std::cout << "步骤2: 执行基于Z高度的统计滤波..." << std::endl;
    std::cout << "搜索半径: " << params_.edge_knn_filter.radius 
              << ", 最小邻居数: " << params_.edge_knn_filter.min_neighbors
              << ", 异常值阈值(标准差倍数): " << params_.edge_knn_filter.outlier_threshold << std::endl;

    // 重新构建KDTree用于邻域搜索
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    kdtree.setInputCloud(edge_cloud_);
    
    std::vector<double> z_deviations;  // 存储每个点Z值与邻域均值的偏差
    std::vector<bool> valid_points(edge_cloud_->points.size(), true);
    
    // 第一步：对每个点计算其Z值与邻域Z均值的偏差
    for (size_t i = 0; i < edge_cloud_->points.size(); ++i)
    {
        const pcl::PointXYZ& point = edge_cloud_->points[i];
        
        std::vector<int> neighbor_indices;
        std::vector<float> neighbor_squared_distances;
        
        // 半径搜索，找到邻域内的点
        int found_neighbors = kdtree.radiusSearch(point, params_.edge_knn_filter.radius,
                                                 neighbor_indices, neighbor_squared_distances);
        
        if (found_neighbors < params_.edge_knn_filter.min_neighbors)
        {
            // 邻居点数量不足，标记为无效点
            valid_points[i] = false;
            z_deviations.push_back(0.0);
            continue;
        }
        
        // 计算邻域内所有点的Z值均值和标准差（包括自身）
        double z_sum = 0.0;
        int valid_neighbor_count = 0;
        
        for (int idx : neighbor_indices)
        {
            z_sum += edge_cloud_->points[idx].z;
            valid_neighbor_count++;
        }
        
        if (valid_neighbor_count == 0)
        {
            valid_points[i] = false;
            z_deviations.push_back(0.0);
            continue;
        }
        
        // 计算邻域Z值均值
        double z_mean = z_sum / valid_neighbor_count;
        
        // 计算邻域Z值标准差
        double z_variance = 0.0;
        for (int idx : neighbor_indices)
        {
            double diff = edge_cloud_->points[idx].z - z_mean;
            z_variance += diff * diff;
        }
        z_variance /= valid_neighbor_count;
        double z_std = std::sqrt(z_variance);
        
        // 计算当前点Z值与邻域均值的偏差（标准化）
        double z_deviation = (z_std > 1e-6) ? std::abs(point.z - z_mean) / z_std : 0.0;
        z_deviations.push_back(z_deviation);
    }
    
    // 第二步：过滤Z值异常的点
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_edge_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    int filtered_count = 0;
    int valid_count = 0;
    double max_deviation = 0.0;
    double avg_deviation = 0.0;
    
    // 计算统计信息
    for (size_t i = 0; i < edge_cloud_->points.size(); ++i)
    {
        if (valid_points[i])
        {
            avg_deviation += z_deviations[i];
            max_deviation = std::max(max_deviation, z_deviations[i]);
            valid_count++;
        }
    }
    
    if (valid_count > 0)
    {
        avg_deviation /= valid_count;
    }
    
    std::cout << "Z高度统计 - 平均偏差: " << avg_deviation 
              << ", 最大偏差: " << max_deviation 
              << ", 异常值阈值: " << params_.edge_knn_filter.outlier_threshold << std::endl;
    
    // 应用Z高度统计滤波
    for (size_t i = 0; i < edge_cloud_->points.size(); ++i)
    {
        if (valid_points[i] && z_deviations[i] <= params_.edge_knn_filter.outlier_threshold)
        {
            filtered_edge_cloud->points.push_back(edge_cloud_->points[i]);
        }
        else
        {
            filtered_count++;
        }
    }
    
    filtered_edge_cloud->width = filtered_edge_cloud->points.size();
    filtered_edge_cloud->height = 1;
    filtered_edge_cloud->is_dense = true;
    
    // 更新边缘点云
    *edge_cloud_ = *filtered_edge_cloud;
    
    std::cout << "基于Z高度的统计滤波完成，过滤了 " << filtered_count << " 个异常点" << std::endl;
    std::cout << "边缘点Z高度统计滤波全部完成，最终保留 " << edge_cloud_->points.size() << " 个边缘点" << std::endl;
    
    return true;
}

// 边缘点平滑滤波和插值处理
bool ShoeSolePathAlgorithm::applyEdgeSmoothing()
{
    if (edge_cloud_->empty())
    {
        std::cerr << "边缘点云为空，无法进行平滑和插值处理" << std::endl;
        return false;
    }

    std::cout << "开始边缘点平滑滤波和插值处理..." << std::endl;
    std::cout << "输入边缘点数量: " << edge_cloud_->points.size() << std::endl;

    // 第一步：平滑滤波
    if (params_.edge_smoothing.enable_smooth_filter)
    {
        std::cout << "步骤1: 执行平滑滤波..." << std::endl;
        std::cout << "平滑邻域点数: " << params_.edge_smoothing.smooth_neighbor_count 
                  << ", 高斯权重标准差: " << params_.edge_smoothing.smooth_weight_sigma << std::endl;

        pcl::PointCloud<pcl::PointXYZ>::Ptr smoothed_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        
        for (size_t i = 0; i < edge_cloud_->points.size(); ++i)
        {
            const pcl::PointXYZ& current_point = edge_cloud_->points[i];
            
            // 收集邻域点（以当前点为中心的前后邻居点）
            std::vector<pcl::PointXYZ> neighbors;
            std::vector<double> weights;
            
            int half_neighbors = params_.edge_smoothing.smooth_neighbor_count / 2;
            
            for (int j = -half_neighbors; j <= half_neighbors; ++j)
            {
                int neighbor_idx = (int)i + j;
                if (neighbor_idx >= 0 && neighbor_idx < (int)edge_cloud_->points.size())
                {
                    neighbors.push_back(edge_cloud_->points[neighbor_idx]);
                    
                    // 计算高斯权重
                    double distance = std::abs(j);
                    double weight = std::exp(-(distance * distance) / 
                                           (2.0 * params_.edge_smoothing.smooth_weight_sigma * params_.edge_smoothing.smooth_weight_sigma));
                    weights.push_back(weight);
                }
            }
            
            if (neighbors.empty())
            {
                smoothed_cloud->points.push_back(current_point);
                continue;
            }
            
            // 加权平均计算平滑后的坐标
            double sum_x = 0.0, sum_y = 0.0, sum_z = 0.0;
            double sum_weights = 0.0;
            
            for (size_t k = 0; k < neighbors.size(); ++k)
            {
                sum_x += neighbors[k].x * weights[k];
                sum_y += neighbors[k].y * weights[k];
                sum_z += neighbors[k].z * weights[k];
                sum_weights += weights[k];
            }
            
            pcl::PointXYZ smoothed_point;
            smoothed_point.x = sum_x / sum_weights;
            smoothed_point.y = sum_y / sum_weights;
            smoothed_point.z = sum_z / sum_weights;
            
            smoothed_cloud->points.push_back(smoothed_point);
        }
        
        smoothed_cloud->width = smoothed_cloud->points.size();
        smoothed_cloud->height = 1;
        smoothed_cloud->is_dense = true;
        
        // 更新边缘点云
        *edge_cloud_ = *smoothed_cloud;
        
        std::cout << "平滑滤波完成，处理了 " << edge_cloud_->points.size() << " 个点" << std::endl;
    }

    std::cout << "边缘点平滑滤波和插值处理全部完成，最终点数: " << edge_cloud_->points.size() << std::endl;
    
    return true;
}

// 对点云按X坐标排序
pcl::PointCloud<pcl::PointXYZ>::Ptr ShoeSolePathAlgorithm::sortPointsByX(
    const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr sorted_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    *sorted_cloud = *cloud;
    
    std::sort(sorted_cloud->points.begin(), sorted_cloud->points.end(),
             [](const pcl::PointXYZ& a, const pcl::PointXYZ& b) {
                 return a.x < b.x;
             });
    
    return sorted_cloud;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr ShoeSolePathAlgorithm::sortPointsByY(
    const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr sorted_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    *sorted_cloud = *cloud;
    
    std::sort(sorted_cloud->points.begin(), sorted_cloud->points.end(),
             [](const pcl::PointXYZ& a, const pcl::PointXYZ& b) {
                 return a.y < b.y;
             });
    
    return sorted_cloud;
}

// 在给定点集中找到极值点
pcl::PointXYZ ShoeSolePathAlgorithm::findExtremePoint(const std::vector<pcl::PointXYZ>& points,
                                                     bool find_max, bool from_top)
{
    if (points.empty())
    {
        return pcl::PointXYZ(0, 0, 0);
    }

    pcl::PointXYZ extreme_point = points[0];
    
    if (from_top)
    {
        // 从顶部开始搜索
        for (const auto& point : points)
        {
            if (find_max)
            {
                if (point.z > extreme_point.z)
                {
                    extreme_point = point;
                }
            }
            else
            {
                if (point.z < extreme_point.z)
                {
                    extreme_point = point;
                }
            }
        }
    }
    else
    {
        // 从底部开始搜索
        for (auto it = points.rbegin(); it != points.rend(); ++it)
        {
            if (find_max)
            {
                if (it->z > extreme_point.z)
                {
                    extreme_point = *it;
                }
            }
            else
            {
                if (it->z < extreme_point.z)
                {
                    extreme_point = *it;
                }
            }
        }
    }
    
    return extreme_point;
}

// 保存滤波后的点云
bool ShoeSolePathAlgorithm::saveFilteredCloud(const std::string& filename)
{
    if (filtered_cloud_->empty())
    {
        std::cerr << "滤波点云为空，无法保存" << std::endl;
        return false;
    }

    if (pcl::io::savePCDFileASCII(filename, *filtered_cloud_) == -1)
    {
        std::cerr << "无法保存滤波点云到: " << filename << std::endl;
        return false;
    }

    std::cout << "成功保存滤波后点云到: " << filename << std::endl;
    return true;
}

// 保存边缘路径点云
bool ShoeSolePathAlgorithm::saveEdgePath(const std::string& filename)
{
    if (edge_cloud_->empty())
    {
        std::cerr << "边缘点云为空，无法保存" << std::endl;
        return false;
    }

    if (pcl::io::savePCDFileASCII(filename, *edge_cloud_) == -1)
    {
        std::cerr << "无法保存边缘点云到: " << filename << std::endl;
        return false;
    }

    std::cout << "成功保存边缘路径到: " << filename << std::endl;
    return true;
}

// 计算法向量（使用参数文件）
// 计算边缘路径的法向量（整合版本，支持参数文件和直接传参）
bool ShoeSolePathAlgorithm::computeEdgeNormals()
{
    return computeEdgeNormals(params_.normal_computation.neighbor_radius, 
                              params_.normal_computation.min_neighbors);
}

bool ShoeSolePathAlgorithm::computeEdgeNormals(double neighbor_radius, int min_neighbors)
{
    if (edge_cloud_->empty())
    {
        std::cerr << "边缘点云为空，无法计算法向量" << std::endl;
        return false;
    }

    if (filtered_cloud_->empty())
    {
        std::cerr << "滤波后点云为空，无法作为法向量计算的参考" << std::endl;
        return false;
    }

    std::cout << "开始计算边缘路径法向量（使用2D PCA算法）..." << std::endl;
    std::cout << "邻域搜索半径: " << neighbor_radius << ", 最小邻居点数: " << min_neighbors << std::endl;

    // 计算鞋底点云的质心，用于确定法向量方向（仅计算XY坐标用于2D方向判断）
    Eigen::Vector3f shoe_centroid(0.0f, 0.0f, 0.0f);
    if (filtered_cloud_->points.size() >  0)
    {
        for (const auto& point : filtered_cloud_->points)
        {
            shoe_centroid[0] += point.x;
            shoe_centroid[1] += point.y;
            shoe_centroid[2] += point.z;
        }
        shoe_centroid /= static_cast<float>(filtered_cloud_->points.size());
        std::cout << "鞋底质心位置: (" << shoe_centroid[0] << ", " 
                  << shoe_centroid[1] << ", " << shoe_centroid[2] << ")" << std::endl;
        std::cout << "注意：法向量计算将仅使用XY平面坐标（2D PCA）" << std::endl;
    }

    edge_normals_.clear();
    edge_normals_.reserve(edge_cloud_->points.size());

    // 构建用于邻域搜索的KDTree
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    kdtree.setInputCloud(filtered_cloud_);

    int successful_normals = 0;

    for (size_t i = 0; i < edge_cloud_->points.size(); ++i)
    {
        const pcl::PointXYZ& edge_point = edge_cloud_->points[i];
        
        // 在滤波后的点云中搜索邻域
        std::vector<int> neighbor_indices;
        std::vector<float> neighbor_distances;
        
        int found_neighbors = kdtree.radiusSearch(edge_point, neighbor_radius, 
                                                 neighbor_indices, neighbor_distances);
        
        if (found_neighbors >= min_neighbors)
        {
            // 收集邻域点
            std::vector<pcl::PointXYZ> neighbors;
            for (int idx : neighbor_indices)
            {
                neighbors.push_back(filtered_cloud_->points[idx]);
            }
            
            // 计算法向量，传入鞋底质心用于方向判断
            pcl::Normal normal = computePointNormal(edge_point, neighbors, shoe_centroid);
            edge_normals_.push_back(normal);
            successful_normals++;
        }
        else
        {
            // 如果邻域点不足，使用默认XY平面法向量 (1, 0, 0)
            pcl::Normal default_normal;
            default_normal.normal_x = 1.0;
            default_normal.normal_y = 0.0;
            default_normal.normal_z = 0.0;  // XY平面法向量，Z分量为0
            default_normal.curvature = 0.0;
            edge_normals_.push_back(default_normal);
            
            std::cout << "警告: 边缘点 " << i << " 邻域点不足(" << found_neighbors 
                     << " < " << min_neighbors << ")，使用默认XY平面法向量 (1,0,0)" << std::endl;
        }
    }

    std::cout << "法向量计算完成（2D PCA模式），成功计算 " << successful_normals 
              << "/" << edge_cloud_->points.size() << " 个法向量" << std::endl;

    // 法向量角度滤波和平滑处理
    if (params_.normal_computation.enable_normal_smoothing)
    {
        std::cout << "开始法向量角度滤波和平滑处理（2D模式）..." << std::endl;
        std::cout << "平滑半径: " << params_.normal_computation.smooth_radius 
                  << ", 角度阈值: " << params_.normal_computation.angle_threshold << "度"
                  << ", 迭代次数: " << params_.normal_computation.smooth_iterations << std::endl;
        
        applyNormalSmoothing();
    }

    return true;
}

// 使用2D PCA方法计算点的法向量，直接在XY平面计算，确保法向量指向边界外侧
pcl::Normal ShoeSolePathAlgorithm::computePointNormal(const pcl::PointXYZ& point,
                                                     const std::vector<pcl::PointXYZ>& neighbors,
                                                     const Eigen::Vector3f& shoe_centroid)
{
    pcl::Normal normal;
    
    if (neighbors.size() < 3)
    {
        // 邻域点太少，返回默认法向量
        normal.normal_x = 1.0;
        normal.normal_y = 0.0;
        normal.normal_z = 0.0;  // XY平面法向量，Z分量为0
        normal.curvature = 0.0;
        return normal;
    }

    // 计算邻域点在XY平面的质心（忽略Z坐标）
    Eigen::Vector2f xy_centroid(0.0f, 0.0f);
    for (const auto& neighbor : neighbors)
    {
        xy_centroid[0] += neighbor.x;
        xy_centroid[1] += neighbor.y;
        // 忽略Z坐标，直接在XY平面进行计算
    }
    xy_centroid /= static_cast<float>(neighbors.size());

    // 构建2D协方差矩阵（仅考虑XY平面）
    Eigen::Matrix2f covariance_matrix_2d = Eigen::Matrix2f::Zero();
    for (const auto& neighbor : neighbors)
    {
        Eigen::Vector2f diff(neighbor.x - xy_centroid[0], 
                            neighbor.y - xy_centroid[1]);
        covariance_matrix_2d += diff * diff.transpose();
    }
    covariance_matrix_2d /= static_cast<float>(neighbors.size() - 1);

    // 进行2D特征值分解
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix2f> eigen_solver_2d(covariance_matrix_2d);
    if (eigen_solver_2d.info() != Eigen::Success)
    {
        // 特征值分解失败，返回默认法向量
        normal.normal_x = 1.0;
        normal.normal_y = 0.0;
        normal.normal_z = 0.0;
        normal.curvature = 0.0;
        return normal;
    }

    // 2D情况下：最小特征值对应的特征向量就是法向量方向
    Eigen::Vector2f eigenvalues_2d = eigen_solver_2d.eigenvalues();
    Eigen::Matrix2f eigenvectors_2d = eigen_solver_2d.eigenvectors();
    
    // 选择最小特征值对应的特征向量作为法向量（在XY平面内）
    int min_eigenvalue_index = 0;
    if (eigenvalues_2d[1] < eigenvalues_2d[0])
    {
        min_eigenvalue_index = 1;
    }
    
    Eigen::Vector2f normal_vector_2d = eigenvectors_2d.col(min_eigenvalue_index);
    
    // 计算从鞋底质心到当前边缘点的向量（仅考虑XY平面）
    Eigen::Vector2f centroid_to_point_2d(point.x - shoe_centroid[0],
                                        point.y - shoe_centroid[1]);
    
    // 确保法向量指向外部（与质心到点的方向一致）
    // 使用2D点积判断方向是否一致
    if (normal_vector_2d.dot(centroid_to_point_2d) < 0)
    {
        normal_vector_2d = -normal_vector_2d;
    }

    // 归一化法向量
    normal_vector_2d.normalize();

    // 直接设置XY平面法向量，Z分量始终为0
    normal.normal_x = normal_vector_2d[0];
    normal.normal_y = normal_vector_2d[1];
    normal.normal_z = 0.0f;  // 固定为0，确保是XY平面法向量
    
    // 计算曲率（2D情况下使用特征值比值）
    float eigenvalue_ratio = (eigenvalues_2d[1] > 0) ? eigenvalues_2d[0] / eigenvalues_2d[1] : 0.0f;
    normal.curvature = eigenvalue_ratio;

    return normal;
}

// 生成内侧打胶点（使用参数文件）
// 根据边缘路径法向量生成内侧打胶点（整合版本，支持参数文件和直接传参）
bool ShoeSolePathAlgorithm::generateInnerGluingPoints()
{
    return generateInnerGluingPoints(params_.inner_gluing_points.offset_distance,
                                     params_.inner_gluing_points.use_projection_height);
}

bool ShoeSolePathAlgorithm::generateInnerGluingPoints(double offset_distance, bool use_projection_height)
{
    if (edge_cloud_->empty() || edge_normals_.empty())
    {
        std::cerr << "边缘点云或法向量为空，无法生成内侧打胶点" << std::endl;
        return false;
    }

    if (edge_cloud_->points.size() != edge_normals_.size())
    {
        std::cerr << "边缘点数量与法向量数量不匹配" << std::endl;
        return false;
    }

    if (filtered_cloud_->empty())
    {
        std::cerr << "滤波后点云为空，无法用作高度计算的参考" << std::endl;
        return false;
    }

    std::cout << "开始生成内侧打胶点，偏移距离: " << offset_distance << " 单位" << std::endl;
    std::cout << "搜索半径: " << params_.inner_gluing_points.search_radius 
              << ", 高度偏移: " << params_.inner_gluing_points.height_offset << std::endl;

    inner_gluing_points_->clear();
    inner_gluing_points_->reserve(edge_cloud_->points.size());

    int successful_points = 0;

    // 为每个边缘点生成对应的内侧打胶点
    for (size_t i = 0; i < edge_cloud_->points.size(); ++i)
    {
        const pcl::PointXYZ& edge_point = edge_cloud_->points[i];
        const pcl::Normal& normal = edge_normals_[i];

        // 检查法向量是否有效
        if (std::isnan(normal.normal_x) || std::isnan(normal.normal_y) || 
            std::isnan(normal.normal_z))
        {
            std::cout << "警告: 边缘点 " << i << " 的法向量无效，跳过" << std::endl;
            continue;
        }

        // 第一步：沿法向量偏移计算内侧点的XY位置
        pcl::PointXYZ inner_point;
        inner_point.x = edge_point.x - normal.normal_x * offset_distance;
        inner_point.y = edge_point.y - normal.normal_y * offset_distance;
        inner_point.z = edge_point.z;  // 临时设为边缘点高度

        // 第二步：直接在内侧点XY位置搜索滤波点云中的最高点
        double max_height = findMaxHeightAroundProjection(inner_point);
        
        // 第三步：在最高点基础上加上偏移量
        inner_point.z = max_height + params_.inner_gluing_points.height_offset;
        inner_gluing_points_->points.push_back(inner_point);
        successful_points++;
    }



    inner_gluing_points_->width = inner_gluing_points_->points.size();
    inner_gluing_points_->height = 1;
    inner_gluing_points_->is_dense = true;

    std::cout << "内侧打胶点生成完成，成功生成 " << successful_points 
              << " 个内侧打胶点" << std::endl;
    return successful_points > 0;
}

// 保存内侧打胶点到PCD文件
bool ShoeSolePathAlgorithm::saveInnerGluingPointsPCD(const std::string& filename)
{
    if (inner_gluing_points_->empty())
    {
        std::cerr << "内侧打胶点为空，无法保存" << std::endl;
        return false;
    }

    if (pcl::io::savePCDFileBinary(filename, *inner_gluing_points_) == -1)
    {
        std::cerr << "保存内侧打胶点PCD文件失败: " << filename << std::endl;
        return false;
    }

    std::cout << "成功保存内侧打胶点到PCD文件: " << filename << std::endl;
    std::cout << "内侧打胶点数量: " << inner_gluing_points_->points.size() << std::endl;
    
    return true;
}

// 保存带法向量的边缘路径到PCD文件
bool ShoeSolePathAlgorithm::saveEdgePathWithNormalsPCD(const std::string& filename)
{
    if (edge_cloud_->empty() || edge_normals_.empty())
    {
        std::cerr << "边缘点云或法向量为空，无法保存" << std::endl;
        return false;
    }

    if (edge_cloud_->points.size() != edge_normals_.size())
    {
        std::cerr << "边缘点数量与法向量数量不匹配" << std::endl;
        return false;
    }

    // 创建包含点和法向量的点云
    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
    cloud_with_normals->width = edge_cloud_->points.size();
    cloud_with_normals->height = 1;
    cloud_with_normals->is_dense = true;
    cloud_with_normals->points.resize(edge_cloud_->points.size());

    // 将边缘点和法向量复制到新的点云结构中
    for (size_t i = 0; i < edge_cloud_->points.size(); ++i)
    {
        pcl::PointNormal& point_normal = cloud_with_normals->points[i];
        
        // 复制点坐标
        point_normal.x = edge_cloud_->points[i].x;
        point_normal.y = edge_cloud_->points[i].y;
        point_normal.z = edge_cloud_->points[i].z;
        
        // 复制法向量
        point_normal.normal_x = edge_normals_[i].normal_x;
        point_normal.normal_y = edge_normals_[i].normal_y;
        point_normal.normal_z = edge_normals_[i].normal_z;
        point_normal.curvature = edge_normals_[i].curvature;
    }

    // 保存到PCD文件
    if (pcl::io::savePCDFileASCII(filename, *cloud_with_normals) == -1)
    {
        std::cerr << "无法保存带法向量的边缘点云到: " << filename << std::endl;
        return false;
    }

    std::cout << "成功保存带法向量的边缘路径PCD文件到: " << filename << std::endl;
    std::cout << "包含 " << cloud_with_normals->points.size() << " 个点和法向量" << std::endl;
    return true;
}

// 打印统计信息
void ShoeSolePathAlgorithm::printStatistics()
{
    std::cout << "\n=== 算法处理统计信息 ===" << std::endl;
    std::cout << "输入点云点数: " << input_cloud_->points.size() << std::endl;
    std::cout << "滤波后点云点数: " << filtered_cloud_->points.size() << std::endl;
    std::cout << "边缘点数: " << edge_cloud_->points.size() << std::endl;
    std::cout << "内侧打胶点数: " << inner_gluing_points_->points.size() << std::endl;
    std::cout << "外侧打胶点数: " << outer_gluing_points_->points.size() << std::endl;
    std::cout << "采样后外部路径点数: " << sampled_outer_path_->points.size() << std::endl;
    std::cout << "采样后内部路径点数: " << sampled_inner_path_->points.size() << std::endl;
    std::cout << "kNN参数 - k: " << knn_k_ << ", 距离阈值: " << distance_thresh_ << std::endl;
    std::cout << "========================\n" << std::endl;
}

// 均匀采样（使用参数文件）
// 均匀采样外部和内部路径（整合版本，支持参数文件和直接传参）
bool ShoeSolePathAlgorithm::uniformSamplePaths()
{
    return uniformSamplePaths(params_.path_sampling.outer_points, params_.path_sampling.inner_points);
}

bool ShoeSolePathAlgorithm::uniformSamplePaths(int outer_points, int inner_points)
{
    if (outer_gluing_points_->empty() || inner_gluing_points_->empty())
    {
        std::cerr << "边缘点云或内侧打胶点云为空，无法进行采样" << std::endl;
        return false;
    }

    std::cout << "开始均匀采样路径点..." << std::endl;
    std::cout << "外部路径目标点数: " << outer_points << ", 内部路径目标点数: " << inner_points << std::endl;

    // 清空采样结果
    sampled_outer_path_->clear();
    sampled_inner_path_->clear();

    //1. 采样外部路径 (outer_gluing_points_)
    int outer_total = outer_gluing_points_->points.size();
    if (outer_total > 0)
    {
        sampled_outer_path_->reserve(outer_points);
        
        if (outer_total <= outer_points)
        {
            // 如果原始点数少于目标点数，直接复制所有点（转换为PointXYZI）
            sampled_outer_path_->points.resize(outer_total);
            for (size_t i = 0; i < outer_total; ++i) {
                const pcl::PointXYZI& pt = outer_gluing_points_->points[i];
                sampled_outer_path_->points[i].x = pt.x;
                sampled_outer_path_->points[i].y = pt.y;
                sampled_outer_path_->points[i].z = pt.z;
                sampled_outer_path_->points[i].intensity = pt.intensity; // 保留原始分区索引
            }
            std::cout << "外部路径原始点数(" << outer_total << ")少于目标点数，直接使用所有点" << std::endl;
        }
        else
        {
            // 先将无序点云排序为连续路径
            std::cout << "正在排序外部路径点云..." << std::endl;
            pcl::PointCloud<pcl::PointXYZI>::Ptr sorted_outer_path = sortPointCloudAsPath(outer_gluing_points_);
            
            // 基于距离的均匀采样
            std::vector<double> cumulative_distances = computeCumulativeDistances(sorted_outer_path);
            double total_distance = cumulative_distances.back();
            
            std::cout << "外部路径总长度: " << total_distance << " mm" << std::endl;
            
            sampled_outer_path_->points.resize(outer_points);
            for (int i = 0; i < outer_points; ++i)
            {
                // 计算目标距离（均匀分布）
                double target_distance;
                if (i == 0) {
                    target_distance = 0.0;  // 第一个点
                } else if (i == outer_points - 1) {
                    target_distance = total_distance;  // 最后一个点
                } else {
                    target_distance = (double(i) / double(outer_points - 1)) * total_distance;
                }
                
                // 根据目标距离找到对应的点索引
                size_t point_index = findPointByDistance(cumulative_distances, target_distance);
                
                const pcl::PointXYZI& pt = sorted_outer_path->points[point_index];
                sampled_outer_path_->points[i].x = pt.x;
                sampled_outer_path_->points[i].y = pt.y;
                sampled_outer_path_->points[i].z = pt.z;
                sampled_outer_path_->points[i].intensity = pt.intensity; // 保留原始分区索引
            }
            std::cout << "外部路径基于距离均匀采样完成" << std::endl;
        }
        
        sampled_outer_path_->width = sampled_outer_path_->points.size();
        sampled_outer_path_->height = 1;
        sampled_outer_path_->is_dense = true;
    }

    //2. 采样内部路径 (inner_gluing_points_)
    int inner_total = inner_gluing_points_->points.size();
    if (inner_total > 0)
    {
        sampled_inner_path_->reserve(inner_points);
        
        if (inner_total <= inner_points)
        {
            // 如果原始点数少于目标点数，直接复制所有点
            *sampled_inner_path_ = *inner_gluing_points_;
            std::cout << "内部路径原始点数(" << inner_total << ")少于目标点数，直接使用所有点" << std::endl;
        }
        else
        {
            // 先将无序点云排序为连续路径
            std::cout << "正在排序内部路径点云..." << std::endl;
            pcl::PointCloud<pcl::PointXYZ>::Ptr sorted_inner_path = sortPointCloudAsPath(inner_gluing_points_);
            
            // 基于距离的均匀采样
            std::vector<double> cumulative_distances = computeCumulativeDistances(sorted_inner_path);
            double total_distance = cumulative_distances.back();
            
            std::cout << "内部路径总长度: " << total_distance << " mm" << std::endl;
            
            sampled_inner_path_->points.clear();
            sampled_inner_path_->points.reserve(inner_points);
            
            for (int i = 0; i < inner_points; ++i)
            {
                // 计算目标距离（均匀分布）
                double target_distance;
                if (i == 0) {
                    target_distance = 0.0;  // 第一个点
                } else if (i == inner_points - 1) {
                    target_distance = total_distance;  // 最后一个点
                } else {
                    target_distance = (double(i) / double(inner_points - 1)) * total_distance;
                }
                
                // 根据目标距离找到对应的点索引
                size_t point_index = findPointByDistance(cumulative_distances, target_distance);
                
                sampled_inner_path_->points.push_back(sorted_inner_path->points[point_index]);
            }
            std::cout << "内部路径基于距离均匀采样完成" << std::endl;
        }
        
        sampled_inner_path_->width = sampled_inner_path_->points.size();
        sampled_inner_path_->height = 1;
        sampled_inner_path_->is_dense = true;
    }

    std::cout << "采样完成 - 外部路径: " << sampled_outer_path_->points.size() 
              << " 点, 内部路径: " << sampled_inner_path_->points.size() << " 点" << std::endl;
    std::cout << "--------------------" << std::endl;
    
    // 注意：新的路径偏移调节逻辑已经在边缘点云处理阶段完成（在applyOuterSprayOffset中）
    // 这里的采样是基于已经处理过偏移的边缘点云进行的

    // 对采样后的路径进行可选的半径滤波和Z高度平滑处理
    if (params_.path_radius_filter.enable_radius_filter) {
        radiusFilterSampledPaths();
    }

    if (params_.path_smoothing.enable_z_smoothing) {
        smoothSampledPaths();
    }

    return true;
}

// 保存采样后的路径
bool ShoeSolePathAlgorithm::saveSampledPaths(const std::string& outer_filename, const std::string& inner_filename)
{
    bool success = true;
    
    // 保存外部路径
    if (!sampled_outer_path_->empty())
    {
        if (pcl::io::savePCDFileASCII(outer_filename, *sampled_outer_path_) == -1)
        {
            std::cerr << "保存采样后外部路径失败: " << outer_filename << std::endl;
            success = false;
        }
        else
        {
            std::cout << "成功保存采样后外部路径: " << outer_filename 
                      << " (" << sampled_outer_path_->points.size() << " 点)" << std::endl;
        }
    }
    
    // 保存内部路径
    if (!sampled_inner_path_->empty())
    {
        if (pcl::io::savePCDFileASCII(inner_filename, *sampled_inner_path_) == -1)
        {
            std::cerr << "保存采样后内部路径失败: " << inner_filename << std::endl;
            success = false;
        }
        else
        {
            std::cout << "成功保存采样后内部路径: " << inner_filename 
                      << " (" << sampled_inner_path_->points.size() << " 点)" << std::endl;
        }
    }
    
    return success;
}


// 对采样后的路径应用标定变换
bool ShoeSolePathAlgorithm::transformSampledPaths()
{
    // 检查是否有路径数据
    if (sampled_outer_path_->empty() && sampled_inner_path_->empty())
    {
        std::cerr << "采样后的路径为空，无法进行变换" << std::endl;
        return false;
    }

    std::cout << "开始应用标定变换..." << std::endl;

    // 提取旋转矩阵用于姿态变换
    Eigen::Matrix3f rotation_matrix = calib_matrix_.block<3,3>(0,0);

    // 清空变换结果
    transformed_outer_path_->clear();
    transformed_inner_path_->clear();
    transformed_outer_path_poses_.clear();
    transformed_inner_path_poses_.clear();

    // 1. 变换外部路径点云
    if (!sampled_outer_path_->empty())
    {
        transformed_outer_path_->reserve(sampled_outer_path_->points.size());
        
        for (const auto& point : sampled_outer_path_->points)
        {
            // 构建齐次坐标
            Eigen::Vector4f homogeneous_point(point.x, point.y, point.z, 1.0f);
            
            // 应用变换矩阵
            Eigen::Vector4f transformed_point = calib_matrix_ * homogeneous_point;
            
            // 转换回PCL点（保持强度值）
            pcl::PointXYZI transformed_pcl_point;
            transformed_pcl_point.x = transformed_point(0);
            transformed_pcl_point.y = transformed_point(1);
            transformed_pcl_point.z = transformed_point(2);
            transformed_pcl_point.intensity = point.intensity; // 保持原始强度值
            
            transformed_outer_path_->points.push_back(transformed_pcl_point);
        }
        
        transformed_outer_path_->width = transformed_outer_path_->points.size();
        transformed_outer_path_->height = 1;
        transformed_outer_path_->is_dense = true;
    }

    // 2. 变换内部路径点云
    if (!sampled_inner_path_->empty())
    {
        transformed_inner_path_->reserve(sampled_inner_path_->points.size());
        
        for (const auto& point : sampled_inner_path_->points)
        {
            // 构建齐次坐标
            Eigen::Vector4f homogeneous_point(point.x, point.y, point.z, 1.0f);
            
            // 应用变换矩阵
            Eigen::Vector4f transformed_point = calib_matrix_ * homogeneous_point;
            
            // 转换回PCL点
            pcl::PointXYZ transformed_pcl_point;
            transformed_pcl_point.x = transformed_point(0);
            transformed_pcl_point.y = transformed_point(1);
            transformed_pcl_point.z = transformed_point(2);
            
            transformed_inner_path_->points.push_back(transformed_pcl_point);
        }
        
        transformed_inner_path_->width = transformed_inner_path_->points.size();
        transformed_inner_path_->height = 1;
        transformed_inner_path_->is_dense = true;
    }

    // 3. 变换外侧路径姿态信息
    for (const auto& pose : outer_path_poses_)
    {
        // 变换位置
        Eigen::Vector4f pos(pose[0], pose[1], pose[2], 1.0f);
        Eigen::Vector4f transformed_pos = calib_matrix_ * pos;
        
        // 变换姿态（旋转角度）
        Eigen::Vector3f euler(pose[3], pose[4], pose[5]); // rx, ry, rz
        
        // 将欧拉角转换为旋转矩阵，然后与标定旋转矩阵相乘
        Eigen::Matrix3f euler_rotation;
        euler_rotation = Eigen::AngleAxisf(euler.z(), Eigen::Vector3f::UnitZ())
                      * Eigen::AngleAxisf(euler.y(), Eigen::Vector3f::UnitY())
                      * Eigen::AngleAxisf(euler.x(), Eigen::Vector3f::UnitX());
        
        // 应用标定旋转
        Eigen::Matrix3f transformed_rotation = rotation_matrix * euler_rotation;
        
        // 将旋转矩阵转换回欧拉角
        Eigen::Vector3f transformed_euler = transformed_rotation.eulerAngles(2, 1, 0); // ZYX顺序
        
        // 存储变换后的姿态
        std::vector<float> transformed_pose(6);
        transformed_pose[0] = transformed_pos.x();
        transformed_pose[1] = transformed_pos.y();
        transformed_pose[2] = transformed_pos.z();
        transformed_pose[3] = transformed_euler.z(); // roll
        transformed_pose[4] = transformed_euler.y(); // pitch
        transformed_pose[5] = transformed_euler.x(); // yaw
        
        transformed_outer_path_poses_.push_back(transformed_pose);
    }

    // 4. 变换内侧路径姿态信息
    for (const auto& pose : inner_path_poses_)
    {
        // 变换位置
        Eigen::Vector4f pos(pose[0], pose[1], pose[2], 1.0f);
        Eigen::Vector4f transformed_pos = calib_matrix_ * pos;
        
        // 变换姿态（旋转角度）
        Eigen::Vector3f euler(pose[3], pose[4], pose[5]); // rx, ry, rz
        
        // 将欧拉角转换为旋转矩阵，然后与标定旋转矩阵相乘
        Eigen::Matrix3f euler_rotation;
        euler_rotation = Eigen::AngleAxisf(euler.z(), Eigen::Vector3f::UnitZ())
                      * Eigen::AngleAxisf(euler.y(), Eigen::Vector3f::UnitY())
                      * Eigen::AngleAxisf(euler.x(), Eigen::Vector3f::UnitX());
        
        // 应用标定旋转
        Eigen::Matrix3f transformed_rotation = rotation_matrix * euler_rotation;
        
        // 将旋转矩阵转换回欧拉角
        Eigen::Vector3f transformed_euler = transformed_rotation.eulerAngles(2, 1, 0); // ZYX顺序
        
        // 存储变换后的姿态
        std::vector<float> transformed_pose(6);
        transformed_pose[0] = transformed_pos.x();
        transformed_pose[1] = transformed_pos.y();
        transformed_pose[2] = transformed_pos.z();
        transformed_pose[3] = transformed_euler.z(); // roll
        transformed_pose[4] = transformed_euler.y(); // pitch
        transformed_pose[5] = transformed_euler.x(); // yaw
        
        transformed_inner_path_poses_.push_back(transformed_pose);
    }

    // 5. 变换全局清扫路径姿态信息（如果存在）
    transformed_global_sweep_path_poses_.clear();
    for (const auto& pose : global_sweep_path_poses_)
    {
        // 变换位置
        Eigen::Vector4f pos(pose[0], pose[1], pose[2], 1.0f);
        Eigen::Vector4f transformed_pos = calib_matrix_ * pos;
        
        // 变换姿态（旋转角度）
        Eigen::Vector3f euler(pose[3], pose[4], pose[5]); // rx, ry, rz
        
        // 将欧拉角转换为旋转矩阵，然后与标定旋转矩阵相乘
        Eigen::Matrix3f euler_rotation;
        euler_rotation = Eigen::AngleAxisf(euler.z(), Eigen::Vector3f::UnitZ())
                      * Eigen::AngleAxisf(euler.y(), Eigen::Vector3f::UnitY())
                      * Eigen::AngleAxisf(euler.x(), Eigen::Vector3f::UnitX());
        
        // 应用标定旋转
        Eigen::Matrix3f transformed_rotation = rotation_matrix * euler_rotation;
        
        // 将旋转矩阵转换回欧拉角
        Eigen::Vector3f transformed_euler = transformed_rotation.eulerAngles(2, 1, 0); // ZYX顺序
        
        // 存储变换后的姿态
        std::vector<float> transformed_pose(6);
        transformed_pose[0] = transformed_pos.x();
        transformed_pose[1] = transformed_pos.y();
        transformed_pose[2] = transformed_pos.z();
        transformed_pose[3] = transformed_euler.z(); // roll
        transformed_pose[4] = transformed_euler.y(); // pitch
        transformed_pose[5] = transformed_euler.x(); // yaw
        
        transformed_global_sweep_path_poses_.push_back(transformed_pose);
    }

    std::cout << "标定变换完成 - 外部路径: " << transformed_outer_path_->points.size() 
              << " 点, 内部路径: " << transformed_inner_path_->points.size() << " 点" << std::endl;
    std::cout << "姿态信息变换完成 - 外部姿态: " << transformed_outer_path_poses_.size()
              << " 个, 内部姿态: " << transformed_inner_path_poses_.size() << " 个" << std::endl;
    if (!transformed_global_sweep_path_poses_.empty()) {
        std::cout << "全局清扫路径变换完成: " << transformed_global_sweep_path_poses_.size() << " 个姿态" << std::endl;
    }

    return true;
}

// 保存变换后的路径(PCD和TXT格式)
bool ShoeSolePathAlgorithm::saveTransformedPaths(const std::string& outer_pcd_filename, 
                                                  const std::string& inner_pcd_filename,
                                                  const std::string& outer_txt_filename, 
                                                  const std::string& inner_txt_filename)
{
    bool success = true;
    
    // 保存外部路径PCD
    if (!transformed_outer_path_->empty())
    {
        if (pcl::io::savePCDFileASCII(outer_pcd_filename, *transformed_outer_path_) == -1)
        {
            std::cerr << "保存变换后外部路径PCD失败: " << outer_pcd_filename << std::endl;
            success = false;
        }
        else
        {
            std::cout << "成功保存变换后外部路径PCD: " << outer_pcd_filename << std::endl;
        }
    }
    
    // 保存内部路径PCD
    if (!transformed_inner_path_->empty())
    {
        if (pcl::io::savePCDFileASCII(inner_pcd_filename, *transformed_inner_path_) == -1)
        {
            std::cerr << "保存变换后内部路径PCD失败: " << inner_pcd_filename << std::endl;
            success = false;
        }
        else
        {
            std::cout << "成功保存变换后内部路径PCD: " << inner_pcd_filename << std::endl;
        }
    }

    // 保存外部路径的变换后姿态信息到TXT文件
    if (!transformed_outer_path_poses_.empty())
    {
        std::ofstream outer_txt_file(outer_txt_filename);
        if (outer_txt_file.is_open())
        {
            outer_txt_file << "# 变换后外部路径点坐标和姿态信息\n";
            outer_txt_file << "# X Y Z RX RY RZ\n";
            
            for (const auto& pose : transformed_outer_path_poses_)
            {
                outer_txt_file << pose[0] << " " << pose[1] << " " << pose[2] << " "
                              << pose[3] << " " << pose[4] << " " << pose[5] << "\n";
            }
            
            outer_txt_file.close();
            std::cout << "成功保存变换后外部路径姿态信息: " << outer_txt_filename << std::endl;
        }
        else
        {
            std::cerr << "保存变换后外部路径姿态信息失败: " << outer_txt_filename << std::endl;
            success = false;
        }
    }

    // 保存内部路径的变换后姿态信息到TXT文件
    if (!transformed_inner_path_poses_.empty())
    {
        std::ofstream inner_txt_file(inner_txt_filename);
        if (inner_txt_file.is_open())
        {
            inner_txt_file << "# 变换后内部路径点坐标和姿态信息\n";
            inner_txt_file << "# X Y Z RX RY RZ\n";
            
            for (const auto& pose : transformed_inner_path_poses_)
            {
                inner_txt_file << pose[0] << " " << pose[1] << " " << pose[2] << " "
                              << pose[3] << " " << pose[4] << " " << pose[5] << "\n";
            }
            
            inner_txt_file.close();
            std::cout << "成功保存变换后内部路径姿态信息: " << inner_txt_filename << std::endl;
        }
        else
        {
            std::cerr << "保存变换后内部路径姿态信息失败: " << inner_txt_filename << std::endl;
            success = false;
        }
    }

    return success;
}

/**
 * @brief 在指定点的XY位置周围搜索最高点
 * @param reference_point 参考点（使用其XY坐标作为搜索中心）
 * @return 周围区域的最高点Z坐标
 */
double ShoeSolePathAlgorithm::findMaxHeightAroundProjection(const pcl::PointXYZ& reference_point)
{
    if (filtered_cloud_->empty())
    {
        std::cerr << "滤波后点云为空，无法搜索最高点" << std::endl;
        return reference_point.z;
    }

    // 第一步：找到与reference_point的XY坐标最接近的filtered_cloud_中的点
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    kdtree.setInputCloud(filtered_cloud_);

    // 先找到最近的几个点
    std::vector<int> nearest_indices;
    std::vector<float> nearest_distances;
    int k = std::min(5000, (int)filtered_cloud_->points.size());
    int found_nearest = kdtree.nearestKSearch(reference_point, k, nearest_indices, nearest_distances);
    
    if (found_nearest == 0)
    {
        return reference_point.z;
    }

    // 在最近的点中找到XY距离最小的点作为投影点
    double min_xy_distance = std::numeric_limits<double>::max();
    pcl::PointXYZ projected_point = reference_point;
    
    for (int idx : nearest_indices)
    {
        const pcl::PointXYZ& candidate = filtered_cloud_->points[idx];
        
        // 计算XY平面上的距离
        double xy_distance = std::sqrt(
            std::pow(candidate.x - reference_point.x, 2) + 
            std::pow(candidate.y - reference_point.y, 2)
        );
        
        if (xy_distance < min_xy_distance)
        {
            min_xy_distance = xy_distance;
            projected_point = candidate;  // 使用filtered_cloud_中的点作为投影点
        }
    }

    // 第二步：以投影点为中心，在指定半径内搜索最高点
    std::vector<int> neighbor_indices;
    std::vector<float> neighbor_distances;
    
    int found_neighbors = kdtree.radiusSearch(projected_point, params_.inner_gluing_points.search_radius,
                                             neighbor_indices, neighbor_distances);
    
    if (found_neighbors == 0)
    {
        // 如果半径内没有找到点，返回投影点的高度
        return projected_point.z;
    }

    // 在找到的邻域点中搜索最高点
    double max_height = -std::numeric_limits<double>::max();
    
    for (int idx : neighbor_indices)
    {
        const pcl::PointXYZ& neighbor_point = filtered_cloud_->points[idx];
        if (neighbor_point.z > max_height)
        {
            max_height = neighbor_point.z;
        }
    }

    // 如果没有找到更高的点，使用投影点的高度
    if (max_height == -std::numeric_limits<double>::max())
    {
        max_height = projected_point.z;
    }

    return max_height;
}

// 法向量平滑处理相关函数实现

/**
 * @brief 应用法向量平滑滤波
 */
void ShoeSolePathAlgorithm::applyNormalSmoothing()
{
    if (edge_normals_.empty() || edge_cloud_->empty())
    {
        std::cerr << "法向量或边缘点云为空，跳过法向量平滑" << std::endl;
        return;
    }

    if (edge_normals_.size() != edge_cloud_->points.size())
    {
        std::cerr << "法向量数量与边缘点数量不匹配，跳过法向量平滑" << std::endl;
        return;
    }

    // 构建边缘点的KDTree用于邻域搜索
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    kdtree.setInputCloud(edge_cloud_);

    std::vector<pcl::Normal> smoothed_normals = edge_normals_;  // 复制原始法向量

    // 进行多次迭代平滑
    for (int iter = 0; iter < params_.normal_computation.smooth_iterations; ++iter)
    {
        std::cout << "法向量平滑迭代 " << (iter + 1) << "/" 
                  << params_.normal_computation.smooth_iterations << " (Jacobi同步更新)" << std::endl;

        // 创建当轮的只读快照，避免Gauss-Seidel式的顺序依赖
        std::vector<pcl::Normal> current_normals = smoothed_normals;
        int points_smoothed_this_iter = 0;

        for (size_t i = 0; i < edge_cloud_->points.size(); ++i)
        {
            const pcl::PointXYZ& current_point = edge_cloud_->points[i];
            
            // 在指定半径内搜索邻域点
            std::vector<int> neighbor_indices;
            std::vector<float> neighbor_distances;
            
            int found_neighbors = kdtree.radiusSearch(current_point, params_.normal_computation.smooth_radius,
                                                     neighbor_indices, neighbor_distances);
            
            if (found_neighbors < 3)  // 至少需要3个点才能进行有效平滑
            {
                continue;  // 保持原始法向量
            }

            // 收集有效邻域点的索引
            std::vector<size_t> valid_neighbor_indices;
            for (int idx : neighbor_indices)
            {
                if (idx != (int)i)  // 排除自身
                {
                    valid_neighbor_indices.push_back((size_t)idx);
                }
            }

            if (!valid_neighbor_indices.empty())
            {
                // 记录平滑前的法向量
                pcl::Normal original_normal = smoothed_normals[i];
                
                // 对当前点的法向量进行平滑，使用当轮的快照数据作为输入
                pcl::Normal smoothed_normal = smoothNormalWithNeighbors(i, valid_neighbor_indices, current_normals);
                
                // 计算平滑前后的角度变化
                double dot_product = original_normal.normal_x * smoothed_normal.normal_x +
                                   original_normal.normal_y * smoothed_normal.normal_y +
                                   original_normal.normal_z * smoothed_normal.normal_z;
                dot_product = std::max(-1.0, std::min(1.0, dot_product));
                double angle_change = std::acos(std::abs(dot_product)) * 180.0 / M_PI;
                
                // 如果角度变化较大，输出调试信息
                if (angle_change > 30.0) {
                    std::cout << "  警告: 点 " << i << " 法向量变化较大: " << angle_change 
                              << "度, 邻域点数: " << valid_neighbor_indices.size() << std::endl;
                    std::cout << "    原始: (" << original_normal.normal_x << ", " 
                              << original_normal.normal_y << ", " << original_normal.normal_z << ")" << std::endl;
                    std::cout << "    平滑后: (" << smoothed_normal.normal_x << ", " 
                              << smoothed_normal.normal_y << ", " << smoothed_normal.normal_z << ")" << std::endl;
                }
                
                smoothed_normals[i] = smoothed_normal;
                points_smoothed_this_iter++;
            }
        }
        
        std::cout << "  本轮平滑了 " << points_smoothed_this_iter << " 个点" << std::endl;
    }

    // 更新边缘法向量
    edge_normals_ = smoothed_normals;
    
    // 确保所有法向量都指向外侧
    enforceOutwardNormalDirection();
    
    std::cout << "法向量平滑处理完成（使用Jacobi同步更新，避免顺序依赖）" << std::endl;
    std::cout << "平滑迭代次数: " << params_.normal_computation.smooth_iterations << std::endl;
    std::cout << "角度阈值: " << params_.normal_computation.angle_threshold << "度" << std::endl;
}



/**
 * @brief 使用邻域法向量对指定点的法向量进行平滑（Jacobi同步更新方式）
 * @param point_index 要平滑的点索引
 * @param neighbor_indices 邻域点索引列表
 * @param input_normals 输入的法向量数组（只读）
 * @return 平滑后的法向量
 */
pcl::Normal ShoeSolePathAlgorithm::smoothNormalWithNeighbors(size_t point_index, 
                                                           const std::vector<size_t>& neighbor_indices,
                                                           const std::vector<pcl::Normal>& input_normals)
{
    const pcl::Normal& current_normal = input_normals[point_index];
    
    // 累计法向量分量和权重
    double sum_x = current_normal.normal_x;
    double sum_y = current_normal.normal_y;
    double sum_z = current_normal.normal_z;
    double total_weight = 1.0;  // 当前点的权重为1
    
    int flipped_neighbors = 0;
    int valid_neighbors = 0;

    // 遍历邻域点
    for (size_t neighbor_idx : neighbor_indices)
    {
        if (neighbor_idx >= input_normals.size())
            continue;
            
        pcl::Normal neighbor_normal = input_normals[neighbor_idx];
        
        // 计算当前法向量与邻域法向量的点积（保留方向信息）
        double dot_product = current_normal.normal_x * neighbor_normal.normal_x +
                            current_normal.normal_y * neighbor_normal.normal_y +
                            current_normal.normal_z * neighbor_normal.normal_z;
        
        // 如果点积为负，说明方向相反，需要翻转邻域法向量以保持方向一致
        if (dot_product < 0.0)
        {
            neighbor_normal.normal_x = -neighbor_normal.normal_x;
            neighbor_normal.normal_y = -neighbor_normal.normal_y;
            neighbor_normal.normal_z = -neighbor_normal.normal_z;
            dot_product = -dot_product;  // 更新点积值
            flipped_neighbors++;
        }
        
        // 计算角度（使用修正后的点积）
        dot_product = std::max(-1.0, std::min(1.0, dot_product));  // 确保在有效范围内
        double angle_radians = std::acos(dot_product);
        double angle_degrees = angle_radians * 180.0 / M_PI;
        
        // 如果角度小于阈值，则参与平滑
        if (angle_degrees <= params_.normal_computation.angle_threshold)
        {
            // 根据角度计算权重：角度越小，权重越大
            double weight = std::cos(angle_radians);  // 直接使用弧度值
            
            sum_x += weight * neighbor_normal.normal_x;
            sum_y += weight * neighbor_normal.normal_y;
            sum_z += weight * neighbor_normal.normal_z;
            total_weight += weight;
            valid_neighbors++;
        }
    }
    
    // 如果翻转的邻域法向量过多，可能存在问题
    if (flipped_neighbors > neighbor_indices.size() / 2) {
        std::cout << "    警告: 点 " << point_index << " 有 " << flipped_neighbors 
                  << "/" << neighbor_indices.size() << " 个邻域法向量被翻转" << std::endl;
    }
    
    // 计算平滑后的法向量
    pcl::Normal smoothed_normal;
    if (total_weight > 0)
    {
        smoothed_normal.normal_x = sum_x / total_weight;
        smoothed_normal.normal_y = sum_y / total_weight;
        smoothed_normal.normal_z = 0.0f;  // 强制Z分量为0，确保是XY平面法向量
        
        // 重新归一化（仅对XY分量）
        double length_xy = std::sqrt(smoothed_normal.normal_x * smoothed_normal.normal_x +
                                    smoothed_normal.normal_y * smoothed_normal.normal_y);
        
        if (length_xy > 1e-6)
        {
            smoothed_normal.normal_x /= length_xy;
            smoothed_normal.normal_y /= length_xy;
            // smoothed_normal.normal_z 保持为0
            
            // 安全检查：确保平滑后的法向量与原始法向量方向基本一致（仅比较XY分量）
            double final_dot = current_normal.normal_x * smoothed_normal.normal_x +
                              current_normal.normal_y * smoothed_normal.normal_y;
            
            // 如果平滑后的法向量与原始法向量方向相反（点积为负），则翻转
            if (final_dot < 0.0) {
                smoothed_normal.normal_x = -smoothed_normal.normal_x;
                smoothed_normal.normal_y = -smoothed_normal.normal_y;
                // smoothed_normal.normal_z 保持为0
                std::cout << "    信息: 点 " << point_index << " 平滑后法向量方向纠正（2D）" << std::endl;
            }
        }
        else
        {
            // 如果长度太小，保持原始法向量
            smoothed_normal = current_normal;
        }
    }
    else
    {
        smoothed_normal = current_normal;
    }
    
    smoothed_normal.curvature = current_normal.curvature;  // 保持原始曲率
    
    return smoothed_normal;
}



/**
 * @brief 计算B样条基函数值
 * @param i 控制点索引
 * @param degree B样条阶数
 * @param t 参数值
 * @param knots 节点向量
 * @return 基函数值
 */
double ShoeSolePathAlgorithm::basisFunction(int i, int degree, double t, const std::vector<double>& knots)
{
    // De Boor算法计算B样条基函数
    if (degree == 0)
    {
        return (t >= knots[i] && t < knots[i + 1]) ? 1.0 : 0.0;
    }
    
    double left = 0.0, right = 0.0;
    
    // 递归计算
    if (knots[i + degree] != knots[i])
    {
        left = (t - knots[i]) / (knots[i + degree] - knots[i]) * basisFunction(i, degree - 1, t, knots);
    }
    
    if (knots[i + degree + 1] != knots[i + 1])
    {
        right = (knots[i + degree + 1] - t) / (knots[i + degree + 1] - knots[i + 1]) * basisFunction(i + 1, degree - 1, t, knots);
    }
    
    return left + right;
}

/**
 * @brief 生成均匀节点向量
 * @param num_control_points 控制点数量
 * @param degree B样条阶数
 * @return 节点向量
 */
std::vector<double> ShoeSolePathAlgorithm::generateKnotVector(int num_control_points, int degree)
{
    int m = num_control_points + degree;  // 节点向量长度 - 1
    std::vector<double> knots(m + 1);
    
    // 生成均匀节点向量（开端和闭端都有重复度为degree+1的节点）
    for (int i = 0; i <= m; ++i)
    {
        if (i <= degree)
        {
            knots[i] = 0.0;
        }
        else if (i >= num_control_points)
        {
            knots[i] = 1.0;
        }
        else
        {
            knots[i] = (double)(i - degree) / (double)(num_control_points - degree);
        }
    }
    
    return knots;
}

/**
 * @brief 对外部路径应用8区域偏移调节
 * 实现新的路径处理逻辑：
 * 1. 重新组织外轮廓路径（从X最大点开始，左脚逆时针，右脚顺时针）
 * 2. 按X区域划分为5个部分
 * 3. 检测连通性并拆分为最多8个路径段
 * 4. 对每个段应用对应的Z偏移和法向量偏移
 * @return 成功返回true，失败返回false
 */
/**
 * @brief 检查两个相邻点是否连通
 * @param current_index 当前点索引
 * @param next_index 下一个点索引
 * @return true表示连通，false表示断开
 */
bool ShoeSolePathAlgorithm::isPointConnectedToNext(size_t current_index, size_t next_index) const
{
    if (current_index >= edge_cloud_->points.size() || 
        next_index >= edge_cloud_->points.size()) {
        return false;
    }
    
    const auto& pt1 = edge_cloud_->points[current_index];
    const auto& pt2 = edge_cloud_->points[next_index];
    
    // 计算两点间的欧式距离
    double distance = std::sqrt(
        std::pow(pt2.x - pt1.x, 2) + 
        std::pow(pt2.y - pt1.y, 2) + 
        std::pow(pt2.z - pt1.z, 2)
    );
    
    // 如果距离超过连通性阈值，认为是断开的
    return distance <= params_.outer_spray_offset.connection_gap_threshold;
}

/**
 * @brief 按X区域将路径划分为5个部分
 * @return 5个区域的路径段，每个段包含点的索引
 */
std::vector<std::vector<size_t>> ShoeSolePathAlgorithm::dividePathByXRegions()
{
    std::vector<std::vector<size_t>> x_regions(5);  // 5个X区域
    
    if (edge_cloud_->empty()) {
        std::cerr << "边缘点云为空，无法进行X区域划分" << std::endl;
        return x_regions;
    }
    
    std::cout << "开始按X坐标将路径划分为5个区域..." << std::endl;
    
    // 计算X坐标范围
    double x_min, x_max;
    x_min = x_max = edge_cloud_->points[0].x;
    
    for (const auto& point : edge_cloud_->points) {
        if (point.x < x_min) x_min = point.x;
        if (point.x > x_max) x_max = point.x;
    }
    
    if (x_max <= x_min) {
        std::cerr << "X坐标范围无效，无法进行区域划分" << std::endl;
        return x_regions;
    }
    
    std::cout << "X坐标范围: [" << x_min << ", " << x_max << "]" << std::endl;
    
    // 获取5个区域的百分比
    const auto& offset_params = params_.outer_spray_offset;
    double region_percentages[5] = {
        offset_params.x_region_percent_1,
        offset_params.x_region_percent_2, 
        offset_params.x_region_percent_3,
        offset_params.x_region_percent_4,
        offset_params.x_region_percent_5
    };
    
    double total_percent = 0.0;
    for (int i = 0; i < 5; ++i) {
        total_percent += region_percentages[i];
    }
    
    if (total_percent <= 0.0) {
        std::cerr << "区域百分比总和为0，无法进行划分" << std::endl;
        return x_regions;
    }
    
    // 计算累积边界
    double x_range = x_max - x_min;
    double cumulative_percent = 0.0;
    std::vector<double> region_boundaries;
    region_boundaries.push_back(x_min);  // 第一个边界
    
    for (int i = 0; i < 4; ++i) {  // 只需要计算前4个区域的右边界
        cumulative_percent += region_percentages[i] / total_percent;
        double boundary = x_min + x_range * cumulative_percent;
        region_boundaries.push_back(boundary);
    }
    region_boundaries.push_back(x_max);  // 最后一个边界
    
    std::cout << "区域边界: ";
    for (size_t i = 0; i < region_boundaries.size(); ++i) {
        std::cout << region_boundaries[i];
        if (i < region_boundaries.size() - 1) std::cout << ", ";
    }
    std::cout << std::endl;
    
    // 将每个点分配到对应的区域
    for (size_t i = 0; i < edge_cloud_->points.size(); ++i) {
        double x_coord = edge_cloud_->points[i].x;
        
        // 确定该点属于哪个区域
        int region = -1;
        for (int r = 0; r < 5; ++r) {
            if (x_coord >= region_boundaries[r] && x_coord <= region_boundaries[r + 1]) {
                region = r;
                break;
            }
        }
        
        // 处理边界情况（避免重复分配）
        if (region == -1) {
            if (x_coord <= region_boundaries[0]) {
                region = 0;  // 分配到第一个区域
            } else if (x_coord >= region_boundaries[5]) {
                region = 4;  // 分配到最后一个区域
            }
        }
        
        if (region >= 0 && region < 5) {
            x_regions[region].push_back(i);
        }
    }
    
    // 输出统计信息
    for (int i = 0; i < 5; ++i) {
        std::cout << "区域" << (i + 1) << ": " << x_regions[i].size() << " 个点 ("
                  << region_percentages[i] << "%)" << std::endl;
    }
    
    return x_regions;
}

/**
 * @brief 拆分和排序路径段，将5个X区域扩展为最多8个连续路径段
 * @param x_regions 按X区域划分的路径段
 * @return 最终的路径段（1-8个），每个段内的点已按要求排序
 */
std::vector<std::vector<size_t>> ShoeSolePathAlgorithm::splitAndSortPathSegments(const std::vector<std::vector<size_t>>& x_regions)
{
    std::vector<std::vector<size_t>> final_segments;
    
    if (x_regions.empty()) {
        std::cerr << "X区域为空，无法进行路径段拆分" << std::endl;
        return final_segments;
    }
    
    std::cout << "开始拆分和排序路径段..." << std::endl;
    
    for (size_t region_idx = 0; region_idx < x_regions.size(); ++region_idx) {
        const auto& region_points = x_regions[region_idx];
        
        if (region_points.empty()) {
            std::cout << "X区域 " << (region_idx + 1) << " 为空，跳过" << std::endl;
            continue;
        }
        
        std::cout << "处理X区域 " << (region_idx + 1) << "，包含 " << region_points.size() << " 个点" << std::endl;
        
        // 对于鞋头鞋尾区域（区域1和5），通常是连接的，作为整体处理
        if (region_idx == 0 || region_idx == 4) {
            std::cout << "  " << (region_idx == 0 ? "鞋头区域" : "鞋尾区域") << "，作为整体路径处理" << std::endl;
            
            // 对区域内的点进行排序（保持原有顺序或按距离排序）
            std::vector<size_t> sorted_region = region_points;
            
            // 按照在原始路径中的顺序排序
            std::sort(sorted_region.begin(), sorted_region.end());
            
            final_segments.push_back(sorted_region);
        }
        // 对于中间区域（区域2、3、4），检查连通性并可能拆分
        else {
            std::cout << "  中间区域，检查连通性..." << std::endl;
            
            // 按原始顺序排序
            std::vector<size_t> sorted_region = region_points;
            std::sort(sorted_region.begin(), sorted_region.end());
            
            // 检测断开点，拆分成多个连续段
            std::vector<std::vector<size_t>> sub_segments;
            std::vector<size_t> current_segment;
            
            for (size_t i = 0; i < sorted_region.size(); ++i) {
                current_segment.push_back(sorted_region[i]);
                
                // 检查是否是最后一个点，或者与下一个点不连通
                bool is_last_point = (i == sorted_region.size() - 1);
                bool is_disconnected = false;
                
                if (!is_last_point) {
                    size_t current_idx = sorted_region[i];
                    size_t next_idx = sorted_region[i + 1];
                    is_disconnected = !isPointConnectedToNext(current_idx, next_idx);
                }
                
                if (is_last_point || is_disconnected) {
                    // 结束当前段
                    if (!current_segment.empty()) {
                        sub_segments.push_back(current_segment);
                        current_segment.clear();
                    }
                }
            }
            
            std::cout << "  区域 " << (region_idx + 1) << " 拆分为 " << sub_segments.size() << " 个子段" << std::endl;
            
            // 对每个子段进行左右排序
            for (size_t sub_idx = 0; sub_idx < sub_segments.size(); ++sub_idx) {
                auto& sub_segment = sub_segments[sub_idx];
                
                if (sub_segment.empty()) continue;
                
                std::cout << "    子段 " << (sub_idx + 1) << ": " << sub_segment.size() << " 个点" << std::endl;
                
                // 根据Y坐标判断是左侧还是右侧路径
                // 计算该段点的平均Y坐标，与整体质心比较
                double avg_y = 0.0;
                for (size_t idx : sub_segment) {
                    avg_y += edge_cloud_->points[idx].y;
                }
                avg_y /= sub_segment.size();
                
                // 计算整体质心的Y坐标
                double centroid_y = 0.0;
                for (const auto& point : edge_cloud_->points) {
                    centroid_y += point.y;
                }
                centroid_y /= edge_cloud_->points.size();
                
                bool is_left_side = (avg_y > centroid_y);
                std::cout << "    该段位于" << (is_left_side ? "左侧" : "右侧") 
                          << " (avg_y=" << avg_y << ", centroid_y=" << centroid_y << ")" << std::endl;
                
                // 根据左右脚类型和段的位置进行排序
                if (params_.outer_spray_offset.is_left_foot) {
                    if (is_left_side) {
                        // 左脚左侧：逆时针（保持原顺序）
                        // 已经按索引顺序排序，无需改变
                    } else {
                        // 左脚右侧：逆时针（可能需要反向）
                        std::reverse(sub_segment.begin(), sub_segment.end());
                    }
                } else {
                    if (is_left_side) {
                        // 右脚左侧：顺时针（可能需要反向）
                        std::reverse(sub_segment.begin(), sub_segment.end());
                    } else {
                        // 右脚右侧：顺时针（保持原顺序）
                        // 已经按索引顺序排序，无需改变
                    }
                }
                
                final_segments.push_back(sub_segment);
            }
        }
    }
    
    // 确保不超过8个段
    if (final_segments.size() > 8) {
        std::cout << "警告: 路径段数量(" << final_segments.size() 
                  << ")超过8个，将合并较小的段" << std::endl;
        
        while (final_segments.size() > 8) {
            // 找到最小的段并与相邻段合并
            size_t min_size = std::numeric_limits<size_t>::max();
            size_t merge_idx = 0;
            
            for (size_t i = 0; i < final_segments.size(); ++i) {
                if (final_segments[i].size() < min_size) {
                    min_size = final_segments[i].size();
                    merge_idx = i;
                }
            }
            
            // 与下一个段合并（如果是最后一个段，则与前一个段合并）
            if (merge_idx < final_segments.size() - 1) {
                final_segments[merge_idx + 1].insert(
                    final_segments[merge_idx + 1].end(),
                    final_segments[merge_idx].begin(),
                    final_segments[merge_idx].end()
                );
            } else if (merge_idx > 0) {
                final_segments[merge_idx - 1].insert(
                    final_segments[merge_idx - 1].end(),
                    final_segments[merge_idx].begin(),
                    final_segments[merge_idx].end()
                );
            }
            
            final_segments.erase(final_segments.begin() + merge_idx);
        }
    }
    
    std::cout << "路径段拆分和排序完成，最终得到 " << final_segments.size() << " 个路径段:" << std::endl;
    for (size_t i = 0; i < final_segments.size(); ++i) {
        std::cout << "  段" << (i + 1) << ": " << final_segments[i].size() << " 个点" << std::endl;
    }
    
    return final_segments;
}


/**
 * @brief 将整个外轮廓路径重新按要求排序
 * 如果是左脚，则从X最大点开始按逆时针排序；如果是右脚，则按顺时针排序
 */
bool ShoeSolePathAlgorithm::reorganizeOuterContourPath()
{
    if (edge_cloud_->empty()) {
        std::cerr << "边缘点云为空，无法重排序" << std::endl;
        return false;
    }
    
    std::cout << "开始重新组织外轮廓路径..." << std::endl;
    std::cout << "脚型设置: " << (params_.outer_spray_offset.is_left_foot ? "左脚" : "右脚") << std::endl;
    std::cout << "输入边缘点数量: " << edge_cloud_->points.size() << std::endl;
    
    // 1. 找到X最大的点作为起始点
    size_t max_x_idx = 0;
    double max_x = edge_cloud_->points[0].x;
    for (size_t i = 1; i < edge_cloud_->points.size(); ++i) {
        if (edge_cloud_->points[i].x > max_x) {
            max_x = edge_cloud_->points[i].x;
            max_x_idx = i;
        }
    }
    
    std::cout << "找到X最大点: 索引=" << max_x_idx << ", X=" << max_x << std::endl;
    
    // 2. 计算路径中心点
    pcl::PointXYZ centroid(0, 0, 0);
    for (const auto& point : edge_cloud_->points) {
        centroid.x += point.x;
        centroid.y += point.y;
        centroid.z += point.z;
    }
    centroid.x /= edge_cloud_->points.size();
    centroid.y /= edge_cloud_->points.size();
    centroid.z /= edge_cloud_->points.size();
    
    std::cout << "路径中心点: (" << centroid.x << ", " << centroid.y << ", " << centroid.z << ")" << std::endl;
    
    // 3. 计算每个点相对于中心的角度
    std::vector<std::pair<double, size_t>> angle_index_pairs;
    const pcl::PointXYZ& start_point = edge_cloud_->points[max_x_idx];
    
    // 计算起始点的角度作为参考
    double start_angle = std::atan2(start_point.y - centroid.y, start_point.x - centroid.x);
    
    for (size_t i = 0; i < edge_cloud_->points.size(); ++i) {
        const pcl::PointXYZ& point = edge_cloud_->points[i];
        double angle = std::atan2(point.y - centroid.y, point.x - centroid.x);
        
        // 调整角度使其相对于起始点
        angle -= start_angle;
        if (angle < 0) angle += 2 * M_PI;
        
        angle_index_pairs.push_back({angle, i});
    }
    
    // 4. 按角度排序
    if (params_.outer_spray_offset.is_left_foot) {
        // 左脚：逆时针（角度从小到大）
        std::sort(angle_index_pairs.begin(), angle_index_pairs.end());
        std::cout << "按逆时针方向排序（左脚）" << std::endl;
    } else {
        // 右脚：顺时针（角度从大到小）
        std::sort(angle_index_pairs.begin(), angle_index_pairs.end(), 
                  [](const std::pair<double, size_t>& a, const std::pair<double, size_t>& b) {
                      return a.first > b.first;
                  });
        std::cout << "按顺时针方向排序（右脚）" << std::endl;
    }
    
    // 5. 重组边缘点云和法向量
    pcl::PointCloud<pcl::PointXYZ>::Ptr new_edge_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    std::vector<pcl::Normal> new_edge_normals;
    
    new_edge_cloud->points.reserve(edge_cloud_->points.size());
    new_edge_normals.reserve(edge_normals_.size());
    
    for (const auto& pair : angle_index_pairs) {
        size_t old_idx = pair.second;
        new_edge_cloud->points.push_back(edge_cloud_->points[old_idx]);
        
        if (old_idx < edge_normals_.size()) {
            new_edge_normals.push_back(edge_normals_[old_idx]);
        }
    }
    
    new_edge_cloud->width = new_edge_cloud->points.size();
    new_edge_cloud->height = 1;
    new_edge_cloud->is_dense = true;
    
    // 更新原始数据
    *edge_cloud_ = *new_edge_cloud;
    edge_normals_ = new_edge_normals;
    
    std::cout << "外轮廓路径重组完成，重排序后点数: " << edge_cloud_->points.size() << std::endl;
    return true;
}

/**
 * @brief 生成外侧打胶点
 * 基于分段后的路径应用不同的Z偏移和法向量偏移生成外侧打胶点
 */
bool ShoeSolePathAlgorithm::generateOuterGluingPoints()
{
    if (edge_cloud_->empty() || edge_normals_.empty()) {
        std::cerr << "边缘点云或法向量为空，无法生成外侧打胶点" << std::endl;
        return false;
    }
    
    if (!params_.outer_spray_offset.enable_offset_adjustment) {
        std::cout << "外部喷胶偏移调节未启用，跳过外侧打胶点生成" << std::endl;
        return false;
    }
    
    std::cout << "开始生成外侧打胶点..." << std::endl;
    
    // 1. 重新组织外轮廓路径
    if (!reorganizeOuterContourPath()) {
        std::cerr << "外轮廓路径重组失败" << std::endl;
        return false;
    }
    
    // 2. 按X区域划分路径
    auto x_regions = dividePathByXRegions();
    if (x_regions.empty()) {
        std::cerr << "X区域划分失败" << std::endl;
        return false;
    }
    
    // 3. 拆分和排序路径段
    auto final_segments = splitAndSortPathSegments(x_regions);
    if (final_segments.empty()) {
        std::cerr << "路径段拆分失败" << std::endl;
        return false;
    }
    
    // 4. 清空外侧打胶点云
    outer_gluing_points_->clear();
    outer_gluing_points_->reserve(edge_cloud_->points.size());
    
    // 5. 获取Z高度偏移和法向量偏移参数
    std::vector<double> z_offsets = {
        params_.outer_spray_offset.z_offset_region_1,
        params_.outer_spray_offset.z_offset_region_2,
        params_.outer_spray_offset.z_offset_region_3,
        params_.outer_spray_offset.z_offset_region_4,
        params_.outer_spray_offset.z_offset_region_5,
        params_.outer_spray_offset.z_offset_region_6,
        params_.outer_spray_offset.z_offset_region_7,
        params_.outer_spray_offset.z_offset_region_8
    };
    
    std::vector<double> normal_offsets = {
        params_.outer_spray_offset.normal_offset_region_1,
        params_.outer_spray_offset.normal_offset_region_2,
        params_.outer_spray_offset.normal_offset_region_3,
        params_.outer_spray_offset.normal_offset_region_4,
        params_.outer_spray_offset.normal_offset_region_5,
        params_.outer_spray_offset.normal_offset_region_6,
        params_.outer_spray_offset.normal_offset_region_7,
        params_.outer_spray_offset.normal_offset_region_8
    };
    
    // 6. 定义8个区域的强度值，用于可视化区分
    std::vector<float> region_intensities = {
        20.0f,  
        50.0f,  
        80.0f, 
        110.0f,  
        140.0f,  
        170.0f,  
        200.0f,  
        230.0f  
    };
    
    // 7. 对每个路径段生成外侧打胶点
    for (size_t seg_idx = 0; seg_idx < final_segments.size(); ++seg_idx) {
        const auto& segment = final_segments[seg_idx];
        size_t region_index = std::min(seg_idx, (size_t)7);  // 确保不超过8个区域
        
        // std::cout << "处理段 " << (seg_idx + 1) << " (区域 " << (region_index + 1) 
        //           << ")，包含 " << segment.size() << " 个点" << std::endl;
        // std::cout << "  Z偏移: " << z_offsets[region_index] 
        //           << ", 法向量偏移: " << normal_offsets[region_index] 
        //           << ", 强度值: " << region_intensities[region_index] << std::endl;
        
        for (size_t point_idx : segment) {
            if (point_idx >= edge_cloud_->points.size() || point_idx >= edge_normals_.size()) {
                std::cerr << "点索引超出范围，跳过" << std::endl;
                continue;
            }
            
            const pcl::PointXYZ& edge_point = edge_cloud_->points[point_idx];
            const pcl::Normal& normal = edge_normals_[point_idx];
            // 生成外侧打胶点
            pcl::PointXYZI outer_point;
            
            // 应用法向量偏移（正值表示向外偏移，负值表示向内偏移）
            outer_point.x = edge_point.x + normal.normal_x * normal_offsets[region_index]*-1;
            outer_point.y = edge_point.y + normal.normal_y * normal_offsets[region_index]*-1;
            outer_point.z = edge_point.z + normal.normal_z * normal_offsets[region_index]*-1;

            // 应用Z高度偏移
            outer_point.z += z_offsets[region_index];
            
            // 设置区域索引值用于分区识别（0-7对应8个分区）
            outer_point.intensity = static_cast<float>(region_index);
            
            outer_gluing_points_->points.push_back(outer_point);
        }
    }
    
    outer_gluing_points_->width = outer_gluing_points_->points.size();
    outer_gluing_points_->height = 1;
    outer_gluing_points_->is_dense = true;
    
    std::cout << "外侧打胶点生成完成，共生成 " << outer_gluing_points_->points.size() << " 个点" << std::endl;
    
    return !outer_gluing_points_->empty();
}



/**
 * @brief 保存外侧打胶点到PCD文件
 */
bool ShoeSolePathAlgorithm::saveOuterGluingPointsPCD(const std::string& filename)
{
    if (outer_gluing_points_->empty()) {
        std::cerr << "外侧打胶点为空，无法保存" << std::endl;
        return false;
    }
    
    if (pcl::io::savePCDFileBinary(filename, *outer_gluing_points_) == -1) {
        std::cerr << "保存外侧打胶点PCD文件失败: " << filename << std::endl;
        return false;
    }
    
    std::cout << "成功保存外侧打胶点到PCD文件: " << filename << std::endl;
    std::cout << "外侧打胶点数量: " << outer_gluing_points_->points.size() << std::endl;
    return true;
}
/**
 * @brief 确保所有边缘法向量都指向外侧
 * 通过与鞋底质心的位置关系来判断和纠正法向量方向（适配2D XY平面法向量）
 */
void ShoeSolePathAlgorithm::enforceOutwardNormalDirection()
{
    if (edge_cloud_->empty() || edge_normals_.empty()) {
        std::cout << "边缘点云或法向量为空，跳过法向量方向约束" << std::endl;
        return;
    }

    std::cout << "开始应用法向量外侧方向约束（2D XY平面模式）..." << std::endl;

    // 计算鞋底点云的质心，用于判断法向量方向（仅使用XY坐标）
    Eigen::Vector2f shoe_centroid_2d(0.0f, 0.0f);
    if (filtered_cloud_->points.size() > 0) {
        for (const auto& point : filtered_cloud_->points) {
            shoe_centroid_2d.x() += point.x;
            shoe_centroid_2d.y() += point.y;
            // 忽略Z坐标，仅使用XY平面
        }
        shoe_centroid_2d /= filtered_cloud_->points.size();
    }

    std::cout << "鞋底质心位置（XY平面）: (" << shoe_centroid_2d.x() << ", " 
              << shoe_centroid_2d.y() << ")" << std::endl;

    int corrected_normals = 0;

    for (size_t i = 0; i < edge_cloud_->points.size(); ++i) {
        const pcl::PointXYZ& edge_point = edge_cloud_->points[i];
        pcl::Normal& normal = edge_normals_[i];

        // 计算从质心到边缘点的向量（XY平面，应该是外侧方向）
        Eigen::Vector2f outward_direction_2d(
            edge_point.x - shoe_centroid_2d.x(),
            edge_point.y - shoe_centroid_2d.y()
        );
        outward_direction_2d.normalize();

        // 计算当前法向量（仅XY分量）
        Eigen::Vector2f current_normal_2d(normal.normal_x, normal.normal_y);
        
        // 检查法向量是否有效
        if (std::isnan(current_normal_2d.x()) || std::isnan(current_normal_2d.y())) {
            // 如果法向量无效，使用外侧方向作为法向量
            normal.normal_x = outward_direction_2d.x();
            normal.normal_y = outward_direction_2d.y();
            normal.normal_z = 0.0f;  // 确保Z分量为0
            corrected_normals++;
            continue;
        }

        current_normal_2d.normalize();

        // 计算法向量与外侧方向的2D点积
        float dot_product = current_normal_2d.dot(outward_direction_2d);

        // 如果点积为负，说明法向量指向内侧，需要翻转（2D情况下更严格：0度阈值）
        if (dot_product < 0.0f) {
            normal.normal_x = -normal.normal_x;
            normal.normal_y = -normal.normal_y;
            normal.normal_z = 0.0f;  // 确保Z分量始终为0
            corrected_normals++;
            std::cout << "  点 " << i << ": 法向量翻转 (2D点积=" << dot_product << ")" << std::endl;
        }
        // 对于点积接近0的情况（法向量与外侧方向垂直），采用更保守的策略
        else if (std::abs(dot_product) < 0.1f) {  // 约6度的容忍角度，在2D平面更敏感
            // 只有当法向量与外侧方向几乎垂直时，才进行轻微调整
            float weight = 0.2f;  // 20%外侧方向，80%原法向量，更保守的权重
            Eigen::Vector2f corrected_normal_2d = weight * outward_direction_2d + (1.0f - weight) * current_normal_2d;
            corrected_normal_2d.normalize();
            
            normal.normal_x = corrected_normal_2d.x();
            normal.normal_y = corrected_normal_2d.y();
            normal.normal_z = 0.0f;  // 确保Z分量始终为0
            corrected_normals++;
            std::cout << "  点 " << i << ": 法向量轻微调整 (2D点积=" << dot_product << ")" << std::endl;
        }
    }

    std::cout << "法向量外侧方向约束完成（2D XY平面模式），纠正了 " << corrected_normals 
              << "/" << edge_normals_.size() << " 个法向量" << std::endl;
}

/**
 * @brief 计算内侧路径点的方向角（x朝向下，y轴朝左）
 * @param point 当前点
 * @param index 当前点在路径中的索引
 * @return 旋转角度（欧拉角，弧度制）
 */
Eigen::Vector3f ShoeSolePathAlgorithm::calculateInnerPathOrientation(const pcl::PointXYZ& point, size_t index)
{
    // 初始化欧拉角（roll, pitch, yaw）
    Eigen::Vector3f euler_angles(0.0f, 0.0f, 0.0f);
    // 如果没有降采样后的内侧路径，返回默认角度
    if (sampled_inner_path_->points.empty()) {
        return euler_angles;
    }
    
    // 按用户要求使用绕轴旋转：
    // 1. 绕z轴顺时针90度
    // 2. 绕y轴顺时针90度
    
    // 创建旋转矩阵
    // 绕z轴顺时针90度 = -π/2 (顺时针为负角度)
    float angle_z = -M_PI / 2.0f;  // -90度
    Eigen::Matrix3f rotation_z;
    rotation_z << std::cos(angle_z), -std::sin(angle_z), 0,
                  std::sin(angle_z),  std::cos(angle_z), 0,
                  0,                  0,                 1;
    
    // 绕y轴顺时针90度 = -π/2 (顺时针为负角度)
    float angle_y = M_PI / 2.0f;  // -90度
    Eigen::Matrix3f rotation_y;
    rotation_y << std::cos(angle_y),  0, std::sin(angle_y),
                  0,                  1, 0,
                  -std::sin(angle_y), 0, std::cos(angle_y);
    
    // 组合旋转：先绕z轴旋转，再绕y轴旋转
    Eigen::Matrix3f combined_rotation =  rotation_z*rotation_y ;
    
    // 将旋转矩阵转换为欧拉角 (Z-Y-X顺序，即yaw-pitch-roll)
    Eigen::Vector3f zyx = combined_rotation.eulerAngles(2, 1, 0);

    euler_angles.z() = zyx[0];  // yaw
    euler_angles.y() = zyx[1];  // pitch
    euler_angles.x() = zyx[2];  // roll

    return euler_angles;
}

/**
 * @brief 计算外侧路径点的方向角（x沿法向量方向，z朝上）
 * @param point 当前点
 * @param index 当前点在路径中的索引
 * @return 旋转角度（欧拉角，弧度制）
 */
Eigen::Vector3f ShoeSolePathAlgorithm::calculateOuterPathOrientation(const pcl::PointXYZI& point, size_t index)
{
    // 初始化欧拉角（roll, pitch, yaw）
    Eigen::Vector3f euler_angles(0.0f, 0.0f, 0.0f);
    
    if (sampled_outer_path_->points.empty() || edge_normals_.empty()) {
        return euler_angles;
    }

    // 为降采样后的点找到最近的原始边缘点的法向量
    Eigen::Vector3f normal_vector(1.0f, 0.0f, 0.0f);  // 默认法向量
    
    if (!edge_cloud_->empty() && !edge_normals_.empty()) {
        // 创建KD树用于最近邻搜索
        pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
        kdtree.setInputCloud(edge_cloud_);
        
        // 将当前点转换为PCL点类型
        pcl::PointXYZ search_point;
        search_point.x = point.x;
        search_point.y = point.y;
        search_point.z = point.z;
        
        // 搜索最近的边缘点
        std::vector<int> point_idx(1);
        std::vector<float> point_squared_distance(1);
        
        if (kdtree.nearestKSearch(search_point, 1, point_idx, point_squared_distance) > 0) {
            const pcl::Normal& nearest_normal = edge_normals_[point_idx[0]];
            
            // 检查法向量是否有效
            if (!std::isnan(nearest_normal.normal_x) && 
                !std::isnan(nearest_normal.normal_y) && 
                !std::isnan(nearest_normal.normal_z)) {
                normal_vector = Eigen::Vector3f(nearest_normal.normal_x, 
                                              nearest_normal.normal_y, 
                                              nearest_normal.normal_z);
                normal_vector.normalize();
            }
        }
    }

    // 根据新要求定义目标坐标系：
    // x与法向量相同，z与点云参考坐标系的z轴相反（即-Z方向）
    Eigen::Vector3f target_x_axis = normal_vector;           // x与法向量相同
    Eigen::Vector3f target_z_axis(0.0f, 0.0f, -1.0f);      // z与参考坐标系z轴相反
    
    // 计算y轴：y = z × x
    Eigen::Vector3f target_y_axis = target_z_axis.cross(target_x_axis);
    
    // 如果法向量与z轴平行，需要特殊处理
    if (target_y_axis.norm() < 1e-6) {
        // 法向量与z轴平行，选择一个垂直方向作为y轴
        target_y_axis = Eigen::Vector3f(0.0f, 1.0f, 0.0f);  // 默认y方向
        target_z_axis = target_x_axis.cross(target_y_axis);
        target_z_axis.normalize();
    } else {
        target_y_axis.normalize();
        // 重新计算z轴确保正交：z = x × y
        target_z_axis = target_x_axis.cross(target_y_axis);
        target_z_axis.normalize();
    }
    
    // 构建目标坐标系的旋转矩阵
    Eigen::Matrix3f target_frame;
    target_frame.col(0) = target_x_axis;
    target_frame.col(1) = target_y_axis;
    target_frame.col(2) = target_z_axis;
    
    // 直接使用点的 intensity 字段作为区域索引（0-7对应8个分区）
    int region_index = static_cast<int>(point.intensity);
    if (region_index < 0 || region_index >= 8) {
        region_index = -1;  // 无效分区索引
    }

    // 如果找到了区域索引，则应用该区域的绕Y轴偏移角（以度为单位），顺时针为正（右手系）
    // 注意：此旋转是相对于每个点的自身局部坐标系进行的，即围绕自身Y轴旋转
    if (region_index >= 0 && region_index < 8) {
        // 使用与 param.yaml 保持一致的参数名 axis_angle_1..axis_angle_8
        double axis_angles_deg[8] = {
            params_.outer_spray_offset.axis_angle_1,
            params_.outer_spray_offset.axis_angle_2,
            params_.outer_spray_offset.axis_angle_3,
            params_.outer_spray_offset.axis_angle_4,
            params_.outer_spray_offset.axis_angle_5,
            params_.outer_spray_offset.axis_angle_6,
            params_.outer_spray_offset.axis_angle_7,
            params_.outer_spray_offset.axis_angle_8
        };

        double angle_deg = axis_angles_deg[region_index];
        double angle_rad = angle_deg * M_PI / 180.0*-1;

        // 构造绕局部Y轴的旋转矩阵（局部坐标系中的绕Y旋转）
        // 此旋转相对于每个点的自身局部坐标系，确保角度偏移是围绕自身Y轴进行的
        Eigen::Matrix3f rotY_local;
        rotY_local << std::cos(angle_rad), 0.0f, std::sin(angle_rad),
                       0.0f, 1.0f, 0.0f,
                      -std::sin(angle_rad), 0.0f, std::cos(angle_rad);

        // 将局部坐标系乘以局部绕Y轴旋转：new_local = local_frame * rotY_local
        target_frame = target_frame * rotY_local;
    }

    // 从旋转矩阵提取欧拉角（ZYX顺序）
    euler_angles.x() = atan2(target_frame(2, 1), target_frame(2, 2));  // Roll
    euler_angles.y() = atan2(-target_frame(2, 0), 
                             sqrt(target_frame(2, 1) * target_frame(2, 1) + 
                                  target_frame(2, 2) * target_frame(2, 2)));  // Pitch
    euler_angles.z() = atan2(target_frame(1, 0), target_frame(0, 0));   // Yaw

    return euler_angles;
}

/**
 * @brief 对采样后的内外路径做Z高度平滑处理
 * 规则：
 * - 对每个点基于其在 `params_.path_smoothing.z_smooth_radius` 半径内的邻域计算Z均值和标准差
 * - 将显著偏离邻域均值（> z_outlier_threshold * std）的点标记为异常点
 * - 在平滑计算时，异常点不作为邻域参考，但异常点本身会用邻域中非异常点的加权平均来更新
 */
void ShoeSolePathAlgorithm::smoothSampledPaths()
{
    std::cout << "开始对采样后的路径进行Z高度平滑处理..." << std::endl;

    // 内外路径都处理，需要KDTree用于空间邻域搜索（使用各自点云）
    // 先处理内部路径（pcl::PointXYZ）
    if (!sampled_inner_path_->empty()) {
        pcl::KdTreeFLANN<pcl::PointXYZ> kdtree_inner;
        kdtree_inner.setInputCloud(sampled_inner_path_);

        int N = sampled_inner_path_->points.size();
        std::vector<double> z_vals(N);
        for (int i = 0; i < N; ++i) z_vals[i] = sampled_inner_path_->points[i].z;

        // 标记异常点（基于局部邻域统计）
        std::vector<bool> is_outlier(N, false);
        for (int i = 0; i < N; ++i) {
            std::vector<int> nb_idx;
            std::vector<float> nb_dist;
            pcl::PointXYZ &p = sampled_inner_path_->points[i];
            int found = kdtree_inner.radiusSearch(p, params_.path_smoothing.z_smooth_radius, nb_idx, nb_dist);
            if (found < 3) { is_outlier[i] = true; continue; }

            double sum = 0; double sum2 = 0;
            int count = 0;
            for (int idx : nb_idx) {
                sum += z_vals[idx];
                sum2 += z_vals[idx]*z_vals[idx];
                ++count;
            }
            if (count <= 0) { is_outlier[i] = true; continue; }
            double mean = sum / count;
            double var = sum2 / count - mean*mean;
            double stddev = (var > 0) ? std::sqrt(var) : 0.0;

            if (std::abs(z_vals[i] - mean) > params_.path_smoothing.z_outlier_threshold * stddev) {
                is_outlier[i] = true;
            }
        }

        // 迭代平滑，异常点不参与对其他点的参考
        for (int iter = 0; iter < params_.path_smoothing.z_smooth_iterations; ++iter) {
            std::vector<double> new_z = z_vals; // snapshot
            int smoothed = 0;
            for (int i = 0; i < N; ++i) {
                pcl::PointXYZ &p = sampled_inner_path_->points[i];
                std::vector<int> nb_idx;
                std::vector<float> nb_dist;
                int found = kdtree_inner.radiusSearch(p, params_.path_smoothing.z_smooth_radius, nb_idx, nb_dist);
                if (found < 1) continue;

                double weighted_sum = 0; double wsum = 0;
                for (int j = 0; j < found; ++j) {
                    int idx = nb_idx[j];
                    if (idx < 0 || idx >= N) continue;
                    // 如果邻域点是异常点，则在参考时跳过
                    if (is_outlier[idx] && idx != i) continue;
                    double d = std::max(1e-6, (double)nb_dist[j]);
                    double weight = 1.0 / (1.0 + d);
                    weighted_sum += z_vals[idx] * weight;
                    wsum += weight;
                }

                if (wsum > 0) {
                    // 异常点也会被平滑：使用邻域非异常点的加权平均
                    if (is_outlier[i]) {
                        new_z[i] = weighted_sum / wsum;
                        smoothed++;
                    } else {
                        // 非异常点也用邻域（不含异常）平滑
                        new_z[i] = weighted_sum / wsum;
                    }
                }
            }
            z_vals.swap(new_z);
        }

        // 写回点云Z
        for (int i = 0; i < N; ++i) sampled_inner_path_->points[i].z = z_vals[i];
        std::cout << "内部路径Z平滑完成: 处理点数=" << sampled_inner_path_->points.size() << std::endl;
    }

    // 再处理外部路径（pcl::PointXYZI）
    if (!sampled_outer_path_->empty()) {
        // 为KDTree创建PointXYZ版本（只使用XYZ用于搜索）
        pcl::PointCloud<pcl::PointXYZ>::Ptr outer_xyz(new pcl::PointCloud<pcl::PointXYZ>);
        outer_xyz->reserve(sampled_outer_path_->points.size());
        for (const auto &pt : sampled_outer_path_->points) {
            pcl::PointXYZ p; p.x = pt.x; p.y = pt.y; p.z = pt.z; outer_xyz->points.push_back(p);
        }

        pcl::KdTreeFLANN<pcl::PointXYZ> kdtree_outer; kdtree_outer.setInputCloud(outer_xyz);
        int N = outer_xyz->points.size();
        std::vector<double> z_vals(N); for (int i = 0; i < N; ++i) z_vals[i] = outer_xyz->points[i].z;
        std::vector<bool> is_outlier(N, false);

        for (int i = 0; i < N; ++i) {
            std::vector<int> nb_idx; std::vector<float> nb_dist;
            int found = kdtree_outer.radiusSearch(outer_xyz->points[i], params_.path_smoothing.z_smooth_radius, nb_idx, nb_dist);
            if (found < 3) { is_outlier[i] = true; continue; }
            double sum = 0, sum2 = 0; int count = 0;
            for (int idx : nb_idx) { sum += z_vals[idx]; sum2 += z_vals[idx]*z_vals[idx]; ++count; }
            if (count <= 0) { is_outlier[i] = true; continue; }
            double mean = sum / count; double var = sum2 / count - mean*mean; double stddev = (var>0)?std::sqrt(var):0.0;
            if (std::abs(z_vals[i] - mean) > params_.path_smoothing.z_outlier_threshold * stddev) is_outlier[i] = true;
        }

        for (int iter = 0; iter < params_.path_smoothing.z_smooth_iterations; ++iter) {
            std::vector<double> new_z = z_vals;
            for (int i = 0; i < N; ++i) {
                std::vector<int> nb_idx; std::vector<float> nb_dist;
                int found = kdtree_outer.radiusSearch(outer_xyz->points[i], params_.path_smoothing.z_smooth_radius, nb_idx, nb_dist);
                if (found < 1) continue;
                double weighted_sum = 0, wsum = 0;
                for (int j = 0; j < found; ++j) {
                    int idx = nb_idx[j]; if (idx<0||idx>=N) continue;
                    if (is_outlier[idx] && idx != i) continue;
                    double d = std::max(1e-6, (double)nb_dist[j]); double weight = 1.0/(1.0+d);
                    weighted_sum += z_vals[idx]*weight; wsum += weight;
                }
                if (wsum>0) new_z[i] = weighted_sum/wsum;
            }
            z_vals.swap(new_z);
        }

        // 写回到 sampled_outer_path_
        for (int i = 0; i < N; ++i) sampled_outer_path_->points[i].z = z_vals[i];
        std::cout << "外部路径Z平滑完成: 处理点数=" << sampled_outer_path_->points.size() << std::endl;
    }

    std::cout << "采样路径Z高度平滑处理完成" << std::endl;
}

// 新增：对采样路径计算角度并保存到 outer_gluing_path_ / inner_gluing_path_
void ShoeSolePathAlgorithm::processPathOrientations()
{
    // 清空之前的姿态信息
    outer_path_poses_.clear();
    inner_path_poses_.clear();

    // 处理外侧路径的姿态信息
    for (size_t i = 0; i < sampled_outer_path_->points.size(); ++i)
    {
        const pcl::PointXYZI& point = sampled_outer_path_->points[i];
        // 计算外侧路径点的方向角（x沿法向量，z朝上）
        Eigen::Vector3f euler = calculateOuterPathOrientation(point, i);
        
        // 存储为[x, y, z, rx, ry, rz]格式
        std::vector<float> pose(6);
        pose[0] = point.x;
        pose[1] = point.y;
        pose[2] = point.z;
        pose[3] = euler.x(); // roll
        pose[4] = euler.y(); // pitch
        pose[5] = euler.z(); // yaw
        
        outer_path_poses_.push_back(pose);
    }

    // 处理内侧路径的姿态信息
    for (size_t i = 0; i < sampled_inner_path_->points.size(); ++i)
    {
        const pcl::PointXYZ& point = sampled_inner_path_->points[i];
        // 计算内侧路径点的方向角（x朝下，y朝左）
        Eigen::Vector3f euler = calculateInnerPathOrientation(point, i);
        
        // 存储为[x, y, z, rx, ry, rz]格式
        std::vector<float> pose(6);
        pose[0] = point.x;
        pose[1] = point.y;
        pose[2] = point.z;
        pose[3] = euler.x(); // roll
        pose[4] = euler.y(); // pitch
        pose[5] = euler.z(); // yaw
        
        inner_path_poses_.push_back(pose);
    }

    std::cout << "路径姿态信息处理完成: 外侧 " << outer_path_poses_.size()
              << " 点, 内侧 " << inner_path_poses_.size() << " 点" << std::endl;
}


/**
 * @brief 对采样后的路径做半径滤波
 * 在每个点为中心在给定半径内查找其它路径点，若存在多个点，则保留与 `filtered_cloud_` 质心最近的一个
 */
void ShoeSolePathAlgorithm::radiusFilterSampledPaths()
{
    if (filtered_cloud_->empty()) {
        std::cout << "filtered_cloud_ 为空，跳过半径滤波" << std::endl;
        return;
    }

    // 计算 filtered_cloud_ 的质心
    Eigen::Vector3f centroid(0,0,0);
    for (const auto &p : filtered_cloud_->points) { centroid.x() += p.x; centroid.y() += p.y; centroid.z() += p.z; }
    centroid /= filtered_cloud_->points.size();

    double radius = params_.path_radius_filter.radius;

    // 外部路径（PointXYZI）
    if (!sampled_outer_path_->empty()) {
        pcl::KdTreeFLANN<pcl::PointXYZI> kdtree;
        kdtree.setInputCloud(sampled_outer_path_);

        std::vector<bool> keep(sampled_outer_path_->points.size(), true);

        for (size_t i = 0; i < sampled_outer_path_->points.size(); ++i) {
            if (!keep[i]) continue;
            pcl::PointXYZI q = sampled_outer_path_->points[i];
            std::vector<int> idxs; std::vector<float> dists;
            int found = kdtree.radiusSearch(q, radius, idxs, dists);
            if (found <= 1) continue;

            // 在这些点中选择与 filtered_cloud_ 质心最近的点保留
            double best_dist = std::numeric_limits<double>::max(); int best_idx = -1;
            for (int id : idxs) {
                double dx = sampled_outer_path_->points[id].x - centroid.x();
                double dy = sampled_outer_path_->points[id].y - centroid.y();
                double dz = sampled_outer_path_->points[id].z - centroid.z();
                double d = dx*dx + dy*dy + dz*dz;
                if (d < best_dist) { best_dist = d; best_idx = id; }
            }

            // 标记其它点为不保留
            for (int id : idxs) {
                if (id != best_idx) keep[id] = false;
            }
        }

        // 构建新的点云
        pcl::PointCloud<pcl::PointXYZI>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZI>);
        for (size_t i = 0; i < sampled_outer_path_->points.size(); ++i) if (keep[i]) filtered->points.push_back(sampled_outer_path_->points[i]);
        *sampled_outer_path_ = *filtered;
        sampled_outer_path_->width = sampled_outer_path_->points.size(); sampled_outer_path_->height = 1;
        std::cout << "外部路径半径滤波完成: 剩余点数=" << sampled_outer_path_->points.size() << std::endl;
    }

    // 内部路径（PointXYZ）
    if (!sampled_inner_path_->empty()) {
        pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
        kdtree.setInputCloud(sampled_inner_path_);

        std::vector<bool> keep(sampled_inner_path_->points.size(), true);

        for (size_t i = 0; i < sampled_inner_path_->points.size(); ++i) {
            if (!keep[i]) continue;
            pcl::PointXYZ q = sampled_inner_path_->points[i];
            std::vector<int> idxs; std::vector<float> dists;
            int found = kdtree.radiusSearch(q, radius, idxs, dists);
            if (found <= 1) continue;

            double best_dist = std::numeric_limits<double>::max(); int best_idx = -1;
            for (int id : idxs) {
                double dx = sampled_inner_path_->points[id].x - centroid.x();
                double dy = sampled_inner_path_->points[id].y - centroid.y();
                double dz = sampled_inner_path_->points[id].z - centroid.z();
                double d = dx*dx + dy*dy + dz*dz;
                if (d < best_dist) { best_dist = d; best_idx = id; }
            }

            for (int id : idxs) {
                if (id != best_idx) keep[id] = false;
            }
        }

        pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>);
        for (size_t i = 0; i < sampled_inner_path_->points.size(); ++i) if (keep[i]) filtered->points.push_back(sampled_inner_path_->points[i]);
        *sampled_inner_path_ = *filtered;
        sampled_inner_path_->width = sampled_inner_path_->points.size(); sampled_inner_path_->height = 1;
        std::cout << "内部路径半径滤波完成: 剩余点数=" << sampled_inner_path_->points.size() << std::endl;
    }
}

// 根据角度对路径进行排序（修改为逆时针路径）
std::vector<std::vector<float>> ShoeSolePathAlgorithm::sortPathByAngle(const std::vector<std::vector<float>>& path_poses)
{
    if (path_poses.empty()) {
        return path_poses;
    }

    if (path_poses.size() == 1) {
        return path_poses;
    }

    // 第一步：找到x极值点作为起始点
    size_t start_index = findMaxXPointIndex(path_poses);  // 找到x最大值
    
    std::vector<std::vector<float>> sorted_path;
    std::vector<bool> visited(path_poses.size(), false);
    
    // 将起始点加入路径
    sorted_path.push_back(path_poses[start_index]);
    visited[start_index] = true;
    
    size_t current_index = start_index;
    float current_x = path_poses[current_index][0];
    float current_y = path_poses[current_index][1];
    
    std::cout << "起始点 (x最大值): (" << current_x << ", " << current_y << ")" << std::endl;
        
    // 第二步：在起始点左侧找到最近点作为第二个点（逆时针方向）
    if (path_poses.size() > 2) {
        std::vector<std::pair<float, size_t>> left_side_distances;
        
        // 只考虑起始点左侧的点（y值小于起始点的点）
        for (size_t i = 0; i < path_poses.size(); ++i) {
            if (visited[i]) continue;
            
            float candidate_y = path_poses[i][1];
            // 只选择左侧的点（y值更小的点）
            if (candidate_y > current_y) {
                float dx = path_poses[i][0] - current_x;
                float dy = path_poses[i][1] - current_y;
                float distance = std::sqrt(dx * dx + dy * dy);
                
                left_side_distances.push_back(std::make_pair(distance, i));
            }
        }
        
        // 如果左侧没有点，则考虑所有未访问点
        if (left_side_distances.empty()) {
            for (size_t i = 0; i < path_poses.size(); ++i) {
                if (visited[i]) continue;
                
                float dx = path_poses[i][0] - current_x;
                float dy = path_poses[i][1] - current_y;
                float distance = std::sqrt(dx * dx + dy * dy);
                
                left_side_distances.push_back(std::make_pair(distance, i));
            }
        }
        
        // 按距离排序，获取最近的点
        if (!left_side_distances.empty()) {
            std::sort(left_side_distances.begin(), left_side_distances.end());
            
            size_t next_index = left_side_distances[0].second;
            sorted_path.push_back(path_poses[next_index]);
            visited[next_index] = true;
            current_index = next_index;
            
            std::cout << "第二点 (左侧最近): (" << path_poses[next_index][0] << ", " << path_poses[next_index][1] << ")" << std::endl;
        }
    }
    
    // 第三步：后续根据x方向距离为参考依次查询最近点
    while (sorted_path.size() < path_poses.size()) {
        current_x = path_poses[current_index][0];
        current_y = path_poses[current_index][1];
        
        std::vector<std::pair<float, size_t>> x_based_scores;
        
        // 计算所有未访问点的x方向为主的评分
        for (size_t i = 0; i < path_poses.size(); ++i) {
            if (visited[i]) continue;
            
            float dx = path_poses[i][0] - current_x;
            float dy = path_poses[i][1] - current_y;
            
            // 计算欧式距离
            // float euclidean_distance = std::sqrt(dx * dx + dy * dy);
            float y_distance = std::abs(dy);
            
            // 计算x方向绝对距离
            float x_distance = std::abs(dx);
            
            // x方向为主的评分：x方向距离占70%，欧式距离占30%
            float x_based_score = x_distance * 0.4f + y_distance * 0.6f;
            
            x_based_scores.push_back(std::make_pair(x_based_score, i));
        }
        
        if (x_based_scores.empty()) {
            break; // 所有点都已访问
        }
        
        // 按x方向评分排序（评分越小越好）
        std::sort(x_based_scores.begin(), x_based_scores.end());
        
        // 选择x方向评分最小的点作为下一个点
        size_t next_index = x_based_scores[0].second;
        
        // 输出调试信息
        float best_score = x_based_scores[0].first;
        // std::cout << "选择点 " << next_index << ": (" << path_poses[next_index][0] 
        //           << ", " << path_poses[next_index][1] << "), x方向评分: " << best_score << std::endl;
        
        sorted_path.push_back(path_poses[next_index]);
        visited[next_index] = true;
        current_index = next_index;
    }
    
    // 第四步：添加起始点作为结束点，形成闭合路径
    sorted_path.push_back(path_poses[start_index]);
    
    std::cout << "逆时针路径排序完成，总点数: " << sorted_path.size() << " (包含闭环连接)" << std::endl;
    
    return sorted_path;
}

// 找到x坐标最小的点的索引（注意：函数名保持兼容性，但实际查找最小值）
size_t ShoeSolePathAlgorithm::findMaxXPointIndex(const std::vector<std::vector<float>>& path_poses)
{
    if (path_poses.empty()) {
        return 0;
    }
    
    size_t max_index = 0;
    float max_x = path_poses[0][0];
    
    for (size_t i = 1; i < path_poses.size(); ++i) {
        if (path_poses[i][0] > max_x) {  // 改为查找最大值
            max_x = path_poses[i][0];
            max_index = i;
        }
    }
    
    return max_index;
}

// 创建全局清扫路径
void ShoeSolePathAlgorithm::createGlobalSweepPath()
{
    global_sweep_path_poses_.clear();
    
    if (outer_path_poses_.empty() && inner_path_poses_.empty()) {
        std::cerr << "外部和内部路径姿态信息都为空，无法创建全局清扫路径" << std::endl;
        return;
    }
    
    std::cout << "开始创建全局清扫路径..." << std::endl;
    
    // 获取左右脚参数
    bool is_left_foot = params_.outer_spray_offset.is_left_foot;
    
    // 对外部路径进行排序
    std::vector<std::vector<float>> sorted_outer_path;
    if (!outer_path_poses_.empty()) {
        sorted_outer_path = sortPathByAngle(outer_path_poses_);
        std::cout << "外部路径排序完成: " << sorted_outer_path.size() << " 点" << std::endl;
    }
    
    // 对内部路径进行排序
    std::vector<std::vector<float>> sorted_inner_path;
    if (!inner_path_poses_.empty()) {
        sorted_inner_path = sortPathByAngle(inner_path_poses_);
        std::cout << "内部路径排序完成: " << sorted_inner_path.size() << " 点" << std::endl;
    }
    
    // 合并路径：先外部喷胶，再内部喷胶
    global_sweep_path_poses_.reserve(sorted_outer_path.size() + sorted_inner_path.size());
    
    // 添加外部路径
    for (const auto& pose : sorted_outer_path) {
        global_sweep_path_poses_.push_back(pose);
    }
    
    // 添加内部路径
    for (const auto& pose : sorted_inner_path) {
        global_sweep_path_poses_.push_back(pose);
    }
    
    std::cout << "全局清扫路径创建完成: 总计 " << global_sweep_path_poses_.size() << " 点" 
              << " (外部: " << sorted_outer_path.size() << " 点, 内部: " << sorted_inner_path.size() << " 点)" << std::endl;
}

// 保存全局清扫路径到TXT文件
bool ShoeSolePathAlgorithm::saveGlobalSweepPath(const std::string& global_sweep_txt_filename)
{
    // 优先保存变换后的全局清扫路径，如果不存在则保存原始路径
    const auto& path_to_save = transformed_global_sweep_path_poses_.empty() ? 
                               global_sweep_path_poses_ : transformed_global_sweep_path_poses_;
    
    if (path_to_save.empty()) {
        std::cerr << "全局清扫路径为空，无法保存" << std::endl;
        return false;
    }
    
    std::ofstream file(global_sweep_txt_filename);
    if (!file.is_open()) {
        std::cerr << "无法打开文件写入: " << global_sweep_txt_filename << std::endl;
        return false;
    }
    
    // 写入文件头
    file << "# 全局清扫路径 (先外部喷胶，再内部喷胶)\n";
    file << "# X Y Z RX RY RZ\n";
    file << "xxxxxxxxxxxxxxxxxxxxxx\n";
    file << "# Total points: " << path_to_save.size() << "\n";
    file << "xxxxxxxxxxxxxxxxxxxxxx\n";

    
    // 写入路径点
    for (const auto& pose : path_to_save) {
        file << pose[0] << "," << pose[1] << "," << pose[2] << ","
             << pose[3] << "," << pose[4] << "," << pose[5] << "\n";
    }
    
    file.close();
    
    std::string path_type = transformed_global_sweep_path_poses_.empty() ? "原始" : "变换后";
    std::cout << "成功保存" << path_type << "全局清扫路径: " << global_sweep_txt_filename 
              << " (" << path_to_save.size() << " 点)" << std::endl;
    
    return true;
}

// 基于距离的均匀采样辅助函数
std::vector<double> ShoeSolePathAlgorithm::computeCumulativeDistances(const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud)
{
    std::vector<double> distances;
    if (cloud->empty()) {
        return distances;
    }
    
    distances.reserve(cloud->points.size());
    distances.push_back(0.0);  // 第一个点距离为0
    
    double cumulative_distance = 0.0;
    for (size_t i = 1; i < cloud->points.size(); ++i) {
        const auto& prev_point = cloud->points[i-1];
        const auto& curr_point = cloud->points[i];
        
        double dx = curr_point.x - prev_point.x;
        double dy = curr_point.y - prev_point.y;
        double dz = curr_point.z - prev_point.z;
        double dist = std::sqrt(dx * dx + dy * dy + dz * dz);
        
        cumulative_distance += dist;
        distances.push_back(cumulative_distance);
    }
    
    return distances;
}

std::vector<double> ShoeSolePathAlgorithm::computeCumulativeDistances(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{
    std::vector<double> distances;
    if (cloud->empty()) {
        return distances;
    }
    
    distances.reserve(cloud->points.size());
    distances.push_back(0.0);  // 第一个点距离为0
    
    double cumulative_distance = 0.0;
    for (size_t i = 1; i < cloud->points.size(); ++i) {
        const auto& prev_point = cloud->points[i-1];
        const auto& curr_point = cloud->points[i];
        
        double dx = curr_point.x - prev_point.x;
        double dy = curr_point.y - prev_point.y;
        double dz = curr_point.z - prev_point.z;
        double dist = std::sqrt(dx * dx + dy * dy + dz * dz);
        
        cumulative_distance += dist;
        distances.push_back(cumulative_distance);
    }
    
    return distances;
}

size_t ShoeSolePathAlgorithm::findPointByDistance(const std::vector<double>& cumulative_distances, double target_distance)
{
    if (cumulative_distances.empty()) {
        return 0;
    }
    
    // 使用二分查找找到最接近目标距离的点
    auto it = std::lower_bound(cumulative_distances.begin(), cumulative_distances.end(), target_distance);
    
    if (it == cumulative_distances.end()) {
        // 目标距离超过了路径长度，返回最后一个点
        return cumulative_distances.size() - 1;
    }
    
    if (it == cumulative_distances.begin()) {
        // 目标距离小于等于0，返回第一个点
        return 0;
    }
    
    // 选择距离目标距离最近的点
    size_t upper_idx = std::distance(cumulative_distances.begin(), it);
    size_t lower_idx = upper_idx - 1;
    
    double upper_dist = std::abs(cumulative_distances[upper_idx] - target_distance);
    double lower_dist = std::abs(cumulative_distances[lower_idx] - target_distance);
    
    return (upper_dist < lower_dist) ? upper_idx : lower_idx;
}

// 点云路径排序函数（将无序点云排序为连续路径）
pcl::PointCloud<pcl::PointXYZI>::Ptr ShoeSolePathAlgorithm::sortPointCloudAsPath(const pcl::PointCloud<pcl::PointXYZI>::Ptr& input)
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr sorted_cloud(new pcl::PointCloud<pcl::PointXYZI>);
    
    if (input->empty()) {
        return sorted_cloud;
    }
    
    if (input->points.size() == 1) {
        *sorted_cloud = *input;
        return sorted_cloud;
    }
    
    // 使用KdTree进行最近邻搜索
    pcl::KdTreeFLANN<pcl::PointXYZI> kdtree;
    kdtree.setInputCloud(input);
    
    std::vector<bool> visited(input->points.size(), false);
    sorted_cloud->points.reserve(input->points.size());
    
    // 选择起始点：这里选择X坐标最小的点作为起始点（适合鞋底轮廓）
    size_t start_idx = 0;
    double min_x = input->points[0].x;
    for (size_t i = 1; i < input->points.size(); ++i) {
        if (input->points[i].x < min_x) {
            min_x = input->points[i].x;
            start_idx = i;
        }
    }
    
    // 从起始点开始构建路径
    size_t current_idx = start_idx;
    sorted_cloud->points.push_back(input->points[current_idx]);
    visited[current_idx] = true;
    
    // 逐步找到最近的未访问点
    for (size_t count = 1; count < input->points.size(); ++count) {
        std::vector<int> pointIdxSearch;
        std::vector<float> pointSquaredDistance;
        
        // 搜索当前点附近的所有点
        if (kdtree.radiusSearch(input->points[current_idx], 1000.0f, pointIdxSearch, pointSquaredDistance) > 0) {
            // 找到最近的未访问点
            double min_distance = std::numeric_limits<double>::max();
            size_t next_idx = current_idx;
            
            for (const int& idx : pointIdxSearch) {
                if (!visited[idx]) {
                    double dist = std::sqrt(pointSquaredDistance[std::find(pointIdxSearch.begin(), pointIdxSearch.end(), idx) - pointIdxSearch.begin()]);
                    if (dist < min_distance) {
                        min_distance = dist;
                        next_idx = idx;
                    }
                }
            }
            
            // 如果没有找到未访问的邻居，搜索所有未访问点中最近的
            if (next_idx == current_idx) {
                min_distance = std::numeric_limits<double>::max();
                for (size_t i = 0; i < input->points.size(); ++i) {
                    if (!visited[i]) {
                        double dx = input->points[i].x - input->points[current_idx].x;
                        double dy = input->points[i].y - input->points[current_idx].y;
                        double dz = input->points[i].z - input->points[current_idx].z;
                        double dist = std::sqrt(dx*dx + dy*dy + dz*dz);
                        if (dist < min_distance) {
                            min_distance = dist;
                            next_idx = i;
                        }
                    }
                }
            }
            
            current_idx = next_idx;
            sorted_cloud->points.push_back(input->points[current_idx]);
            visited[current_idx] = true;
        }
    }
    
    sorted_cloud->width = sorted_cloud->points.size();
    sorted_cloud->height = 1;
    sorted_cloud->is_dense = true;
    
    return sorted_cloud;
}

//增加socket状态上报功能
void ShoeSolePathAlgorithm::socket_send_code(){
    YAML::Node config = YAML::LoadFile("../param.yaml");
    std::string code = config["socket"]["code"].as<std::string>();
    std::string ip = config["socket"]["ip"].as<std::string>();
    int port = config["socket"]["port"].as<int>();

    //创建socket
    int sock = socket(AF_INET, SOCK_DGRAM, 0);
    if(sock ==-1){
        std::cerr << "Socket创建失败" << std::endl;
        return;
    }
    // 设置服务器地址结构
    sockaddr_in server_addr{};
    server_addr.sin_family = AF_INET;
    server_addr.sin_port = htons(port);
    if (inet_pton(AF_INET, ip.c_str(), &server_addr.sin_addr) <= 0) {
        std::cerr << "Invalid IP address: " << ip << std::endl;
        close(sock);
        return ;
    }
    // 连接到服务器
    if (connect(sock, (struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) {
        std::cerr << "Connection failed to " << ip << ":" << port << std::endl;
        close(sock);
        return ;
    }
    // 构造 JSON 数据
    json jsonData;
    jsonData["cameraStatus"] = true;
    jsonData["type"] = 3;
    jsonData["code"] = code;
    jsonData["success"] = true;
    // jsonData["timestamp"] = std::time(nullptr);

    char buffer[20]; // 足够容纳 ISO 8601 格式的时间字符串
    time_t now = std::time(nullptr);
    tm* gmtm = gmtime(&now); // 转换为 UTC 时间
    strftime(buffer, sizeof(buffer), "%Y-%m-%dT%H:%M:%SZ", gmtm); // 格式化为 ISO 8601
    jsonData["timestamp"] = buffer;
    
    jsonData["id"] = 1001;

    std::cout<<"ip:"<<ip<<",port:"<<port<<std::endl;
    // 序列化为字符串并发送
    std::string jsonStr = jsonData.dump(4);
    if (send(sock, jsonStr.c_str(), jsonStr.size(), 0) < 0) {
        std::cerr << "Send failed." << std::endl;
    } else {
        std::cout << "Sent JSON:\n" << jsonStr << std::endl;
    }

    // 关闭 Socket
    close(sock);
}

pcl::PointCloud<pcl::PointXYZ>::Ptr ShoeSolePathAlgorithm::sortPointCloudAsPath(const pcl::PointCloud<pcl::PointXYZ>::Ptr& input)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr sorted_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    
    if (input->empty()) {
        return sorted_cloud;
    }
    
    if (input->points.size() == 1) {
        *sorted_cloud = *input;
        return sorted_cloud;
    }
    
    // 使用KdTree进行最近邻搜索
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    kdtree.setInputCloud(input);
    
    std::vector<bool> visited(input->points.size(), false);
    sorted_cloud->points.reserve(input->points.size());
    
    // 选择起始点：这里选择X坐标最小的点作为起始点（适合鞋底轮廓）
    size_t start_idx = 0;
    double min_x = input->points[0].x;
    for (size_t i = 1; i < input->points.size(); ++i) {
        if (input->points[i].x < min_x) {
            min_x = input->points[i].x;
            start_idx = i;
        }
    }
    
    // 从起始点开始构建路径
    size_t current_idx = start_idx;
    sorted_cloud->points.push_back(input->points[current_idx]);
    visited[current_idx] = true;
    
    // 逐步找到最近的未访问点
    for (size_t count = 1; count < input->points.size(); ++count) {
        std::vector<int> pointIdxSearch;
        std::vector<float> pointSquaredDistance;
        
        // 搜索当前点附近的所有点
        if (kdtree.radiusSearch(input->points[current_idx], 1000.0f, pointIdxSearch, pointSquaredDistance) > 0) {
            // 找到最近的未访问点
            double min_distance = std::numeric_limits<double>::max();
            size_t next_idx = current_idx;
            
            for (const int& idx : pointIdxSearch) {
                if (!visited[idx]) {
                    double dist = std::sqrt(pointSquaredDistance[std::find(pointIdxSearch.begin(), pointIdxSearch.end(), idx) - pointIdxSearch.begin()]);
                    if (dist < min_distance) {
                        min_distance = dist;
                        next_idx = idx;
                    }
                }
            }
            
            // 如果没有找到未访问的邻居，搜索所有未访问点中最近的
            if (next_idx == current_idx) {
                double min_distance = std::numeric_limits<double>::max();
                for (size_t i = 0; i < input->points.size(); ++i) {
                    if (!visited[i]) {
                        double dx = input->points[i].x - input->points[current_idx].x;
                        double dy = input->points[i].y - input->points[current_idx].y;
                        double dz = input->points[i].z - input->points[current_idx].z;
                        double dist = std::sqrt(dx*dx + dy*dy + dz*dz);
                        if (dist < min_distance) {
                            min_distance = dist;
                            next_idx = i;
                        }
                    }
                }
            }
            
            current_idx = next_idx;
            sorted_cloud->points.push_back(input->points[current_idx]);
            visited[current_idx] = true;
        }
    }
    
    sorted_cloud->width = sorted_cloud->points.size();
    sorted_cloud->height = 1;
    sorted_cloud->is_dense = true;
    
    return sorted_cloud;
}
